perm filename PUI2S.PUI[2,TES] blob
sn#009887 filedate 1972-07-20 generic text, type T, neo UTF8
14⎇⎇July 20, 1972␈→60⎇-1000⎇⎇␈←
15⎇⎇␈→60⎇0⎇⎇ 16414⎇␈←␈→60⎇-1000⎇⎇␈←
16⎇⎇␈→60⎇0⎇⎇1.0 INTRODUCTION␈←
17⎇⎇1.1 SYSTEM DESCRIPTION
18⎇⎇We are concerened here with a computer controlled arm and
19⎇⎇hand. This arm and hand function together as a general
20⎇⎇purpose manipulator which forms part of a robot as an
21⎇⎇effector subsystem.
22⎇⎇The computer plans and executes sequences of arm and hand
23⎇⎇motions to accomplish tasks. Arm motions are along smooth,
24⎇⎇collision free, space trajectories; all known forces are
25⎇⎇predicted during planning and compensated for during
26⎇⎇execution. Hand motions which consist of opening and closing
27⎇⎇the hand are controlled by a touch sense. The hand can also
28⎇⎇exert a force while following an externally defined motion.
29⎇⎇Any robot must contain both a sensory input mechanism,
30⎇⎇whereby it can gain information about the environment, and
31⎇⎇an effector subsystem by which it can change the
32⎇⎇environment. In addition to being the main effector, the
33⎇⎇hand also has primitive touch sensors and force detection
34⎇⎇ability, and may be considered a sensory subsystem.
35⎇⎇The environment in which the robot works must be one which
36⎇⎇it can represent internally. It has certain information a
37⎇⎇priori, and it is able to gain more information by
38⎇⎇interacting with its environment.
39⎇⎇In our case it knows, a priori, that objects are plane-
40⎇⎇bounded, solid and may be colored. It knows that all objects
41⎇⎇are supported by a plane, or by each other and that objects
42⎇⎇may be moved, but not through each other. It is given
43⎇⎇prototypes of all the possible objects that can exist, and
44⎇⎇learns of the existence and position of instances of these
45⎇⎇prototypes. Work is in progress to increase the scope of the
46⎇⎇robot's environment; curved objects are currently being
47⎇⎇added [Agin].
48⎇⎇The type of interaction between robot subsystems is
49⎇⎇important. Currently all subsystems perform under the
50⎇⎇direction of the strategy subsystem, with little interaction
51⎇⎇between other subsystems. For instance the arm does not call
52⎇⎇for vision to locate an object that it has dropped, nor does
53⎇⎇vision call the arm to move an obscuring object in order
54⎇⎇July 20, 1972␈→60⎇-1000⎇⎇1.1 SYSTEM␈←
55⎇⎇␈→60⎇0⎇⎇ 16417⎇␈←␈→60⎇-1000⎇⎇␈←
56⎇⎇that it may "see" better. One important exception is the
57⎇⎇case of visual feedback used to position the hand on an
58⎇⎇object, here the vision subsystem interacts directly with
59⎇⎇the arm [Gill].
60⎇⎇To illustrate the system we will describe the interaction to
61⎇⎇solve the Instant Insanity puzzle [Feldman]. Here it is
62⎇⎇first required that four colored cubes be found, and that
63⎇⎇the color of the faces be determined. The cubes are then
64⎇⎇turned and stacked so that each side of the stack has four
65⎇⎇different colored faces visible.
66⎇⎇The interaction between the STRATEGY subsystem, VISION,
67⎇⎇RECOGNIZE, COLOR and the arm is on the following level. The
68⎇⎇STRATEGY subsystem asks the VISION subsystem to find an
69⎇⎇outline and then RECOGNIZE to identify the outline as a
70⎇⎇cube, this process is repeated until four cubes have been
71⎇⎇found. COLOR is then told to find the colors at the center
72⎇⎇of each of the faces. The ARM is told to turn over each cube
73⎇⎇and VISION called to refind the outline; RECOGNIZE is then
74⎇⎇called to reidentify the outline as a cube. When all four
75⎇⎇cubes have been turned over and re-found, COLOR is told to
76⎇⎇find the colors on all the now visible back faces. The
77⎇⎇STRATEGY subsystem now determines the necessary turns and
78⎇⎇stack positions of each of the cubes to obtain a solution.
79⎇⎇The ARM is then told to move each cube accordingly. There
80⎇⎇are two levels of error recovery, within the subsystem, and
81⎇⎇by the stategy subsystem when a subsystem reports faiure.
82⎇⎇The programming environment in which the robot operates is
83⎇⎇known as the "Hand Eye" system [Feldman]. Here individual
84⎇⎇subsystems are represented by time sharing jobs, which may
85⎇⎇interact together. This is done by two means: 1) a global
86⎇⎇data structure which represents information about the
87⎇⎇environment available to all subsystems; 2) The message
88⎇⎇procedure construct whereby one job may execute a procedure
89⎇⎇in another job. For example, cubes are moved by a message
90⎇⎇procedure "MOVE␈π_INSTANCE" which in the Instant Insanity case
91⎇⎇was used both to turn the cubes over and to stack them.
92⎇⎇The arm subsystem provides a series of functions which are
93⎇⎇of general utility, such as to move the arm to a given
94⎇⎇position "MOVE", or to open the hand "OPEN". In order to
95⎇⎇perform these functions the arm subsystem maintains a data
96⎇⎇base in the form of constants and procedures, which describe
97⎇⎇the arm and hand in detail. If the arm is required to
98⎇⎇perform some task then an attempt is made to describe the
99⎇⎇July 20, 1972␈→60⎇-1000⎇⎇1.1 SYSTEM␈←
100⎇⎇␈→60⎇0⎇⎇ 16418⎇␈←␈→60⎇-1000⎇⎇␈←
101⎇⎇task in terms of existing arm functions, if this can be done
102⎇⎇then the problem is solved. If a task is proposed that
103⎇⎇cannot be solved in terms of existing functions, then either
104⎇⎇a function is modified or a new function is added.
105⎇⎇Other subsystems are not expected to use the arm's data
106⎇⎇base,which is local to the arm subsystem. Problem solving
107⎇⎇requiring use of this data base would be considered a
108⎇⎇function of the arm subsystem. Global variables are
109⎇⎇maintained which describe the current state of the arm for
110⎇⎇the other subsystems, in particular "ARM␈π_MOTION" which is
111⎇⎇true when the arm is moving, serves as a warning to other
112⎇⎇subsystems, notably vision, that the environment is
113⎇⎇changing.
114⎇⎇To build a stack of blocks would not be an arm function, as
115⎇⎇it could be accomplished in terms of simpler, existing
116⎇⎇functions. To turn a cube over would be an arm function as
117⎇⎇this requires that the range throughout which the hand can
118⎇⎇grasp the cube be considered. To move the arm through a
119⎇⎇cluttered space of objects is an arm task as this requires
120⎇⎇that the arm be considered in relation to the other objects
121⎇⎇when planning the trajectory that the arm will follow.
122⎇⎇Information is given to the arm subsystem in the function
123⎇⎇call and in the form of a three dimensional description of
124⎇⎇the space. If this space is completely described and the
125⎇⎇arm fails to accomplish a task then the strategy subsystem
126⎇⎇knows that the task must be specified differently. For
127⎇⎇instance, if a block must be moved but some obstacle is in
128⎇⎇the way, then the strategy program must first ask the arm to
129⎇⎇move the obstruction and then ask that the original move be
130⎇⎇accomplished. The arm subsystem would not move other objects
131⎇⎇and thus change the state of the environment without being
132⎇⎇told to do so by the strategy subsystem. If the the space
133⎇⎇model is incomplete and the arm cannot accomplish a task
134⎇⎇based on the available information, then the arm will not
135⎇⎇call the vision subsystem to have this space investigated
136⎇⎇but will report failure.
137⎇⎇There are two main parts to the arm subsystem: the planning
138⎇⎇program and the arm execution program. In order to move an
139⎇⎇object a plan must be made, and the object must be
140⎇⎇considered in relation to the hand to determine how it may
141⎇⎇be grasped. Other objects are considered in relation to the
142⎇⎇hand as it is moved in order to prevent collisions. This
143⎇⎇plan is represented in terms of a coordinated time dependent
144⎇⎇July 20, 1972␈→60⎇-1000⎇⎇1.1 SYSTEM␈←
145⎇⎇␈→60⎇0⎇⎇ 16419⎇␈←␈→60⎇-1000⎇⎇␈←
146⎇⎇trajectory for each joint of the arm. As the computer has
147⎇⎇planned a trajectory the program knows before the arm is
148⎇⎇moved the configuration, velocity and acceleration of all
149⎇⎇the links of the arm and can compute the effective inertia
150⎇⎇and the gravity torque of each link. These terms together
151⎇⎇with the trajectory are given to the arm program, where the
152⎇⎇inertia and gravity terms are used to improve the execution
153⎇⎇of the trajectory. As the gravity and inertia terms may be
154⎇⎇computed during planning, they need not be computed in real
155⎇⎇time.
156⎇⎇The arm servo program executes the trajectory by moving the
157⎇⎇arm, it also performs such actions as opening and closing
158⎇⎇the hand. Trajectories together with hand actions are
159⎇⎇written out in a file, and may be repeatedly executed by the
160⎇⎇arm program if required. The arm program is small and is
161⎇⎇suitable for execution in a mini-computer directly related
162⎇⎇to the arm. The planning program can be run under time
163⎇⎇sharing and can make plans for many such arms.
164⎇⎇The servo is a conventional sampled data servo executed by
165⎇⎇the computer with the following modification: certain
166⎇⎇control constants, the loop gain, predicted gravity and
167⎇⎇external torques are varied with arm configuration.
168⎇⎇In addition to the needs of the current vision and strategy
169⎇⎇subsystems, the arm has been programmed to perform other
170⎇⎇tasks such as turning cranks, screwing in screws and pushing
171⎇⎇and pulling. The touch sense is used in some of these tasks.
172⎇⎇Arm programs may be witten where the course of execution of
173⎇⎇the program by the arm may be modified depending on
174⎇⎇activation of the touch sensors or other conditions. Such
175⎇⎇programs may be written in a form of assembly language but
176⎇⎇are identical to the message procedure calls of a strategy
177⎇⎇program.
178⎇⎇In subsequent sections of this thesis we first describe the
179⎇⎇model of the environment and the hand's interaction with it
180⎇⎇(Section 432⎇). We then describe the model of the arm and
181⎇⎇derive all the relations that we will use (Section 762⎇).
182⎇⎇Section 79⎇ describes MOVE␈π_INSTANCE the highest level
183⎇⎇strategy function of the arm subsystem, primarily provided
184⎇⎇for strategy subsystems performing operations with blocks.
185⎇⎇Is Section 175⎇ we describe the requirements and solution of
186⎇⎇the smooth trajectories used by the arm. The servo loop is
187⎇⎇described and we then deal with control(Section 548⎇),
188⎇⎇giving a list of the arm functions or primitives. A final
189⎇⎇July 20, 1972␈→60⎇-1000⎇⎇1.1 SYSTEM␈←
190⎇⎇␈→60⎇0⎇⎇ 16420⎇␈←␈→60⎇-1000⎇⎇␈←
191⎇⎇section deals with an investigation of the possible motion
192⎇⎇of an unknown object. Here the arm tries to move the object
193⎇⎇in various ways and reports the resultant motion. Analysis
194⎇⎇of this reported data allows the resulting motion to be
195⎇⎇classified.
196⎇⎇The arm is described fully in [Scheinman], and we give a
197⎇⎇brief description in Appendix 946⎇. Two other appendices are
198⎇⎇given Appendix 353⎇ describes SAIL a form of ALGOL with LEAP
199⎇⎇added, which is the language that the programs are written
200⎇⎇in, and which we will use to illustrate some of the
201⎇⎇algorithmns. Appendix 684⎇ briefly covers homogeneous
202⎇⎇coordinate vectors and transformations, more fully covered
203⎇⎇in [ ].
204⎇⎇1.2 HISTORY
205⎇⎇July 20, 1972␈→60⎇-1000⎇⎇␈←
206⎇⎇␈→60⎇0⎇⎇ 16422⎇␈←␈→60⎇-1000⎇⎇␈←
207⎇⎇␈→60⎇0⎇⎇2.0 WORLD MODEL␈←
208⎇⎇This section describes the model of the arm's environment
209⎇⎇which consists of solid plane-faced objects. These are the
210⎇⎇objects that the vision system can identify [Falk] and it is
211⎇⎇necessary to maintain a representation of them. This is done
212⎇⎇both for the Arm program and for the Vision program, which
213⎇⎇share a common data base [Paul].
214⎇⎇We describe the prototype representation and the manner of
215⎇⎇specifying instances of these prototypes. The hand
216⎇⎇coordinates are then defined and the problem of grasping
217⎇⎇this class of objects is reduced to finding a set of
218⎇⎇orientation vectors.
219⎇⎇2.1 PROTOTYPE DESCRIPTION
220⎇⎇We will start our description of an object by first
221⎇⎇describing its prototype. The prototype is located with its
222⎇⎇center of mass at the origin and its principal inertial axes
223⎇⎇aligned with the coordinate axes. Each vertex is represented
224⎇⎇by a vector giving its distance from the origin and each
225⎇⎇face is represented by a row matrix giving its position and
226⎇⎇outward pointing normal.
227⎇⎇Items are created for each part of a prototype (face,
228⎇⎇vertex, edge). Those not familiar with "items" should
229⎇⎇consult Appendix 353⎇. The spacial information associated
230⎇⎇with vertices and faces is stored as array datums of these
231⎇⎇items. Each of these items is associated with the
232⎇⎇prototype, which is itself an item. Different attributes
233⎇⎇are used to indicate which part value is being associated.
234⎇⎇For example in the case of the cube shown in Figure 2.1.
235⎇⎇␈→60⎇0⎇⎇FACE ⊗ CUBE ≡ F1␈←␈→60⎇-1000⎇⎇[Eq. 2.1]␈←
236⎇⎇␈→60⎇0⎇⎇FACE ⊗ CUBE ≡ F2␈←
237⎇⎇␈→60⎇0⎇⎇" " "␈←
238⎇⎇␈→60⎇0⎇⎇FACE ⊗ CUBE ≡ F6␈←
239⎇⎇␈→60⎇0⎇⎇VERTEX ⊗ CUBE ≡ V1␈←␈→60⎇-1000⎇⎇[Eq. 2.2]␈←
240⎇⎇␈→60⎇0⎇⎇VERTEX ⊗ CUBE ≡ V2␈←
241⎇⎇␈→60⎇0⎇⎇" " "␈←
242⎇⎇␈→60⎇0⎇⎇VERTEX ⊗ CUBE ≡ V8␈←
243⎇⎇␈→60⎇0⎇⎇EDGE ⊗ CUBE ≡ E1␈←␈→60⎇-1000⎇⎇[Eq. 2.3]␈←
244⎇⎇July 20, 1972␈→60⎇-1000⎇⎇2.1 PROTOTYPE␈←
245⎇⎇␈→60⎇0⎇⎇ 16426⎇␈←␈→60⎇-1000⎇⎇␈←
246⎇⎇␈→60⎇0⎇⎇EDGE ⊗ CUBE ≡ E2␈←
247⎇⎇␈→60⎇0⎇⎇" " "␈←
248⎇⎇␈→60⎇0⎇⎇EDGE ⊗ CUBE ≡ E12␈←
249⎇⎇In the case of edges the length of the edge is kept as its
250⎇⎇datum.
251⎇⎇For each Face we now associate its vertices and edges
252⎇⎇together
253⎇⎇Consider for example face F1:
254⎇⎇␈→60⎇0⎇⎇BOUNDARY ⊗ F1 ≡ E1␈←␈→60⎇-1000⎇⎇[Eq. 2.4]␈←
255⎇⎇␈→60⎇0⎇⎇BOUNDARY ⊗ F1 ≡ E2␈←
256⎇⎇␈→60⎇0⎇⎇BOUNDARY ⊗ F1 ≡ E3␈←
257⎇⎇␈→60⎇0⎇⎇BOUNDARY ⊗ F1 ≡ E4␈←
258⎇⎇␈→60⎇0⎇⎇CORNER ⊗ F1 ≡ V1␈←␈→60⎇-1000⎇⎇[Eq. 2.5]␈←
259⎇⎇␈→60⎇0⎇⎇CORNER ⊗ F1 ≡ V2␈←
260⎇⎇␈→60⎇0⎇⎇CORNER ⊗ F1 ≡ V3␈←
261⎇⎇␈→60⎇0⎇⎇CORNER ⊗ F1 ≡ V4␈←
262⎇⎇And for edges we associate the edge with its endpoints:
263⎇⎇␈→60⎇0⎇⎇ENDPT ⊗ E1 ≡ V1␈←␈→60⎇-1000⎇⎇[Eq. 2.6]␈←
264⎇⎇␈→60⎇0⎇⎇ENDPT ⊗ E1 ≡ V2␈←
265⎇⎇etc.
266⎇⎇This completes the topological description of a prototype.
267⎇⎇Prototypes are kept for the objects shown in Figure 2.2,
268⎇⎇this data is kept in the global data store and is available
269⎇⎇to all programs.
270⎇⎇Based on such a description it is possible to find many
271⎇⎇relationships, for example:
272⎇⎇July 20, 1972␈→60⎇-1000⎇⎇2.1 PROTOTYPE␈←
273⎇⎇␈→60⎇0⎇⎇ 16428⎇␈←␈→60⎇-1000⎇⎇␈←
274⎇⎇ Given any face F1 find its neighbors which share a
275⎇⎇ common vertex V1.
276⎇⎇ We can find the faces which share the common vertex
277⎇⎇ V1 by finding all the faces which satisfy
278⎇⎇ ␈→60⎇8⎇⎇FOREACH F | VERTEX ⊗ F ≡ V1␈←
279⎇⎇ Here F represents an item variable satisfied by the
280⎇⎇ association. The set of these F's are all the faces
281⎇⎇ which share this common vertex. However they are
282⎇⎇ not all neighbors of F1. To ascertain that they are
283⎇⎇ neighbors we must require that they share a common
284⎇⎇ edge and that they are not F1.
285⎇⎇ FOREACH F | VERTEX ⊗ F ≡ V1
286⎇⎇ ∧ EDGE ⊗ F ≡ E
287⎇⎇ ∧ EDGE ⊗ F1 ≡ E
288⎇⎇ ∧ F ≠ F1
289⎇⎇ This specifies the set of faces { F } which are the
290⎇⎇ neighbors of F1 and share the common vertex V1.
291⎇⎇Another example: it is required to go around the vertices of
292⎇⎇a face (F1) in order, starting with V1
293⎇⎇ T ← V1;
294⎇⎇ FOREACH E,H | EDGE ⊗ F1 ≡ E
295⎇⎇ ∧ END ⊗ E ≡ T
296⎇⎇ ∧ END ⊗ E ≡ H
297⎇⎇ ∧ H ≠ T DO BEGIN
298⎇⎇ IF H = V1 THEN DONE;
299⎇⎇ <statement>;
300⎇⎇ T ← H END;
301⎇⎇When an instance is located a new item is created which is
302⎇⎇associated with the prototype as follows
303⎇⎇␈→60⎇0⎇⎇INSTANCE ⊗ CUBE ≡ INST1␈←␈→60⎇-1000⎇⎇[Eq. 2.7]␈←
304⎇⎇In this "prototype - instance" scheme all information common
305⎇⎇to the instances is kept only once, with the prototype. The
306⎇⎇individual information, the instance transform, is kept with
307⎇⎇each instance. The position information relates to the
308⎇⎇prototype, which is located at the origin.
309⎇⎇July 20, 1972␈→60⎇-1000⎇⎇2.1 PROTOTYPE␈←
310⎇⎇␈→60⎇0⎇⎇ 16429⎇␈←␈→60⎇-1000⎇⎇␈←
311⎇⎇Most calculations can be performed by transforming the
312⎇⎇instance back to the prototype rather than transforming the
313⎇⎇prototype out to the instance.
314⎇⎇Consider for example the problem of finding the support face
315⎇⎇of a body by finding through which face the weight vector
316⎇⎇passes. We could transform each face out to the instance
317⎇⎇and calculate if the weight vector was in the general
318⎇⎇direction of the outward pointing normal and then see if the
319⎇⎇weight vector actually passed through the face. This would
320⎇⎇require that we transform each vertex of the face to the
321⎇⎇instance. It is more efficient to transform the weight
322⎇⎇vector back to the prototype by using the inverse transform
323⎇⎇and check through which face it passed, avoiding all the
324⎇⎇other transformations, which take of the order of 0.5 m sec.
325⎇⎇each. This completes our description of the "prototype -
326⎇⎇instance" scheme, and in the next section we will discuss
327⎇⎇the use of this world model by the arm system.
328⎇⎇2.2 ORIENTATION VECTORS
329⎇⎇The prototype discription is used when it is required to
330⎇⎇move an instance of some prototype. Apart from the
331⎇⎇positional information which is obtainable directly from the
332⎇⎇instance transform, the prototype description is used to
333⎇⎇calculate how the instance may be picked up. Knowing how a
334⎇⎇body is oriented and where it is located does not specify a
335⎇⎇hand position which may be used in order to pick the
336⎇⎇instance up.
337⎇⎇The position and orientation of the hand are described the
338⎇⎇same as for any other object, with the exception of the
339⎇⎇location of the origin of coordinates. The origin of the
340⎇⎇coordinate system in the hand is not at the center of mass
341⎇⎇as in the case of a body, but is located centrally between
342⎇⎇the finger tips with the z axis pointing in the direction of
343⎇⎇the fingers. The y axis is directed across, between the
344⎇⎇fingers.
345⎇⎇To specify a hand position we must give the cartesian
346⎇⎇coordinates of a point centrally located between the finger
347⎇⎇tips P␈-1⎇␈π_. We then give the orientation vector which points
348⎇⎇from one finger tip to the other O␈-1⎇␈π_. Finally, we generate a
349⎇⎇reference approach vector RA␈-2⎇␈π_␈π_ formed by crossing a vertical
350⎇⎇vector with the orientation vector, and then specify the
351⎇⎇July 20, 1972␈→60⎇-1000⎇⎇2.2 ORIENTATIONS␈←
352⎇⎇␈→60⎇0⎇⎇ 16431⎇␈←␈→60⎇-1000⎇⎇␈←
353⎇⎇approach angle between the fingers and this reference
354⎇⎇direction measured about the orientation vector. This gives
355⎇⎇us the approach vector A␈-1⎇␈π_. See Figure 2.3.
356⎇⎇This information can be used to construct the 4 x 4
357⎇⎇transformation which locates the hand as:
358⎇⎇␈→60⎇0⎇⎇| (O x A)[x] O[x] A[x] P[x]|␈←
359⎇⎇␈→60⎇0⎇⎇| |␈←
360⎇⎇␈→60⎇0⎇⎇| (O x A)[y] O[y] A[y] P[y]|␈←
361⎇⎇␈→60⎇0⎇⎇| |␈←
362⎇⎇␈→60⎇0⎇⎇| (O x A)[z] O[z] A[z] P[z]|␈←
363⎇⎇␈→60⎇0⎇⎇| |␈←
364⎇⎇␈→60⎇0⎇⎇| 0 0 0 1 |␈←
365⎇⎇␈→60⎇0⎇⎇␈←␈→60⎇-1000⎇⎇[Eq. 2.8]␈←
366⎇⎇Having described how a hand position is specified we must
367⎇⎇now make some specifications about picking up objects. For
368⎇⎇planar or convex surfaces an object may be picked up by two
369⎇⎇parallel faces on an axis containing the center of mass;
370⎇⎇this will prevent the object from rotating. One but not both
371⎇⎇surfaces may be replaced by an apex of the body. Both
372⎇⎇surfaces may be replaced by edges if a normal from the edge
373⎇⎇intersects the center of mass. These considerations define
374⎇⎇the set of orientation vectors. if the hand is positioned
375⎇⎇at the center of mass with one of these orientation vectors,
376⎇⎇it will be in a position to grasp the object. See Figure
377⎇⎇2.4.
378⎇⎇To find systematically all the possible orientation vectors
379⎇⎇we first find all the contact vectors from the center of
380⎇⎇mass of the object that intersect and are 1) normal to any
381⎇⎇edge, 2) normal to any plane, 3) pass through any apex. We
382⎇⎇then search this list for anti-parallel vectors, being
383⎇⎇careful not to take both vectors from the third class. this
384⎇⎇is done in the following manner:
385⎇⎇For faces we simply check that the perpendicular from the
386⎇⎇plane to the center of mass lies inside the face and thus
387⎇⎇the surface is perpendicular at the contact point
388⎇⎇ FOREACH F | FACE⊗PROTOTYPE≡F DO
389⎇⎇ BEGIN
390⎇⎇ D←DATUM(F)[4];
391⎇⎇ IF D>0.0 THEN GO TO NFACE;
392⎇⎇ if we use this plane the center of mass will
393⎇⎇ not lie between the finger tips
394⎇⎇July 20, 1972␈→60⎇-1000⎇⎇2.2 ORIENTATIONS␈←
395⎇⎇␈→60⎇0⎇⎇ 16434⎇␈←␈→60⎇-1000⎇⎇␈←
396⎇⎇ IF CONTAINED(DATUM(F),F) THEN FOUND ONE;
397⎇⎇ is the point of intersection of the normal
398⎇⎇ and the face within the boundary?
399⎇⎇NFACE: END;
400⎇⎇The procedure CONTAINED counts the number of region boundary
401⎇⎇crossings of a ray from the point to infinity. If the number
402⎇⎇is odd then the point is inside the region, if even then it
403⎇⎇is outside.
404⎇⎇Let us now look at edges. It is necessary that a normal
405⎇⎇vector from the edge pass through the center of mass.
406⎇⎇ FOREACH E,A,B|EDGE⊗PROTOTYPE≡E
407⎇⎇ ∧ END⊗E≡A
408⎇⎇ ∧ END⊗E≡B
409⎇⎇ ∧ A≠B DO BEGIN
410⎇⎇ T←A␈-1⎇␈π_ . (A␈-1⎇␈π_ - B␈-1⎇␈π_)/(A␈-1⎇␈π_ - B␈-1⎇␈π_) . (A␈-1⎇␈π_ - B␈-1⎇␈π_);
411⎇⎇ T is the directed distance from end point A
412⎇⎇ to the normal, divided by the directed
413⎇⎇ distance of A from B.
414⎇⎇ IF T≤0.0 ∨ T≥1.0 THEN GO TO NEDGE;
415⎇⎇ The normal intersects on the edge.
416⎇⎇ C␈-1⎇␈π_ ← A␈-1⎇␈π_ - ((A␈-1⎇␈π_ - B␈-1⎇␈π_) * T);
417⎇⎇ C is a vector from the center of mass and
418⎇⎇ perpendicular to the edge at the point of contact.
419⎇⎇ See Figure 2.5
420⎇⎇ We now check that this is an outside edge:
421⎇⎇ FOREACH N1,N2|BOUNDARY⊗N1≡E
422⎇⎇ ∧ BOUNDARY⊗N2≡E
423⎇⎇ ∧ N1≠N1 DO;
424⎇⎇ N1␈-2⎇␈π_␈π_ ← DATUM(N1);
425⎇⎇ N2␈-2⎇␈π_␈π_ ← DATUM(N2);
426⎇⎇ N␈-1⎇␈π_ ← N1␈-2⎇␈π_␈π_ X N2␈-2⎇␈π_␈π_;
427⎇⎇ N␈-1⎇␈π_ is a refrence vector such that we move
428⎇⎇ outside as we rotate about N␈-1⎇␈π_ from N1␈-2⎇␈π_␈π_ to N2␈-2⎇␈π_␈π_.
429⎇⎇ V␈-1⎇␈π_ ← N1␈-2⎇␈π_␈π_ X C␈-1⎇␈π_;
430⎇⎇ IF N␈-1⎇␈π_ . V␈-1⎇␈π_ ≤ 0
431⎇⎇ C␈-1⎇␈π_ points to the outside of the vertex.
432⎇⎇July 20, 1972␈→60⎇-1000⎇⎇2.2 ORIENTATIONS␈←
433⎇⎇␈→60⎇0⎇⎇ 16436⎇␈←␈→60⎇-1000⎇⎇␈←
434⎇⎇ THEN FOUND ONE;
435⎇⎇ END;
436⎇⎇ END;
437⎇⎇In the caes of vertices it is only the outside corners of
438⎇⎇the object that we can use. See Figure 2.6 That is for all
439⎇⎇edges at this vertex the angle O␈-1⎇- must be less than 90
440⎇⎇degrees. See Figure 2.6.
441⎇⎇ FOREACH V | VERTEX ⊗ BDY ≡ V DO BEGIN
442⎇⎇ C2←V␈-1⎇␈π_ . V␈-1⎇␈π_;
443⎇⎇ FOREACH E,A | END ⊗ E ≡ V
444⎇⎇ ∧ END ⊗ E ≡ A
445⎇⎇ ∧ A ≠ V DO
446⎇⎇ IF A␈-1⎇␈π_ . V␈-1⎇␈π_ > C2 THEN GO TO NOGOOD;
447⎇⎇ FOUND ONE;
448⎇⎇NOGOOD: END;
449⎇⎇We then search this list of contact points looking for anti-
450⎇⎇parallel vectors, being careful not to take both vectors
451⎇⎇from the class of vertices. These then are the orientation
452⎇⎇vectors and are stored with the prototype, together with the
453⎇⎇contact information:
454⎇⎇␈→60⎇0⎇⎇ORIENTATION ⊗ BDY ≡ O1␈←␈→60⎇-1000⎇⎇[Eq. 2.9]␈←
455⎇⎇␈→60⎇0⎇⎇ORIENTATION ⊗ BDY ≡ O2␈←
456⎇⎇␈→60⎇0⎇⎇" " "␈←
457⎇⎇␈→60⎇0⎇⎇ORIENTATION ⊗ BDY ≡ O9␈←
458⎇⎇␈→60⎇0⎇⎇CONTACT ⊗ O1 ≡ F1␈←␈→60⎇-1000⎇⎇[Eq. 2.10]␈←
459⎇⎇␈→60⎇0⎇⎇CONTACT ⊗ O1 ≡ F3␈←
460⎇⎇␈→60⎇0⎇⎇CONTACT ⊗ O2 ≡ F2␈←
461⎇⎇␈→60⎇0⎇⎇CONTACT ⊗ O2 ≡ F4␈←
462⎇⎇␈→60⎇0⎇⎇" " "␈←
463⎇⎇July 20, 1972␈→60⎇-1000⎇⎇2.2 ORIENTATIONS␈←
464⎇⎇␈→60⎇0⎇⎇ 16438⎇␈←␈→60⎇-1000⎇⎇␈←
465⎇⎇␈→60⎇0⎇⎇CONTACT ⊗ O9 ≡ E6␈←
466⎇⎇␈→60⎇0⎇⎇CONTACT ⊗ O9 ≡ E2␈←
467⎇⎇The contact information is used in determining which
468⎇⎇orientation vectors can be used for a given instance. The
469⎇⎇datum of the orientation vector is a 5 element matrix with
470⎇⎇the following elements: If ␈_C1␈≡ and ␈_C2␈≡ are two anti-parallel
471⎇⎇contact vectors then the datum of the orientation vector is:
472⎇⎇␈→60⎇0⎇⎇O[1] = C1[1] / ( C1[1]↑2 + C1[2]↑2 + C1[3]↑2 )␈↑(1/2)␈↓␈←
473⎇⎇␈→60⎇0⎇⎇O[2] = C1[2] / ( C1[1]↑2 + C1[2]↑2 + C1[3]↑2 )␈↑(1/2)␈↓␈←
474⎇⎇␈→60⎇0⎇⎇O[3] = C1[3] / ( C1[1]↑2 + C1[2]↑2 + C1[3]↑2 )␈↑(1/2)␈↓␈←
475⎇⎇␈→60⎇0⎇⎇O[4] = | C1␈-2⎇␈π_␈π_ |␈←
476⎇⎇␈→60⎇0⎇⎇O[5] = | C2␈-2⎇␈π_␈π_ |␈←
477⎇⎇␈→60⎇0⎇⎇O[6] = | O[4] | + | O[5] |␈←
478⎇⎇All arm operations with bodies can be reduced to
479⎇⎇manipulations of these vectors. With such orientation
480⎇⎇vectors associated with each prototype the problem of
481⎇⎇picking up an instance can be reduced to the following: Each
482⎇⎇orientation vector is transformed out to the instance and
483⎇⎇the orientation vectors which contact the support face,
484⎇⎇based on the contact associations, discarded. The instance
485⎇⎇can then be picked up by any one of the remaining
486⎇⎇orientation vectors. When an approach angle is specified the
487⎇⎇hand transform may be specified. The problem of selecting
488⎇⎇the corect orientation vector and approach angle is
489⎇⎇discussed fully in Subsection 310⎇.
490⎇⎇July 20, 1972␈→60⎇-1000⎇⎇␈←
491⎇⎇␈→60⎇0⎇⎇ 16439⎇␈←␈→60⎇-1000⎇⎇␈←
492⎇⎇␈→60⎇0⎇⎇3.0 ARM MODEL␈←
493⎇⎇In this section we will consider the arm and develop a model
494⎇⎇for it [Piper]. We will describe the solution which, given
495⎇⎇a hand position, returns a set of joint angles. Then, based
496⎇⎇on the model of the arm, we will develop a solution for
497⎇⎇differential motion. We will then derive the relation
498⎇⎇between acceleration and force for the arm [Kahn], to obtain
499⎇⎇the effective link inertia and gravity loading. Finally we
500⎇⎇derive the relationship between a force and moment acting at
501⎇⎇hand and the six joint reaction torques. The results of
502⎇⎇this section are used in later sections of the work but as
503⎇⎇they all relate to the arm model they are derived together
504⎇⎇here.
505⎇⎇3.1 KINEMATICS
506⎇⎇The arm shown in Figure 3.1 is a six degree of freedom
507⎇⎇device allowing the hand to be positioned anywhere and with
508⎇⎇any orientation. The position and orientation of any link
509⎇⎇is described by a four by four transformation in the same
510⎇⎇manner as an instance.
511⎇⎇The prototype description for the arm's links is not given
512⎇⎇as we have no need for it, although each link could have a
513⎇⎇prototype associated with it.
514⎇⎇Each link is connected to the next link by a joint. There
515⎇⎇are two kinds of joints, prismatic, or sliding, and
516⎇⎇revolute. In order to describe the link transformation in
517⎇⎇terms of the joint constraint and the joint variable we will
518⎇⎇introduce a coordinate system in which the joint constraint
519⎇⎇is implicit.
520⎇⎇We will develop the "A" matrices which relate between link
521⎇⎇coordinate systems and the "T" matrices, the link
522⎇⎇transformations which specify the position and orientation
523⎇⎇of each link in space. In the case of links the origin of
524⎇⎇coordinates is not the center of mass but is specified at
525⎇⎇the joint axis.
526⎇⎇Associated with each link is an orthogonal coordinate system
527⎇⎇fixed in the link (see Figure 3.2).
528⎇⎇For link i the Zi axis is directed along the axis of the
529⎇⎇July 20, 1972␈→60⎇-1000⎇⎇3.1 KINEMATICS␈←
530⎇⎇␈→60⎇0⎇⎇ 16444⎇␈←␈→60⎇-1000⎇⎇␈←
531⎇⎇joint between link i and i+1. The xi axis is along the
532⎇⎇common normal between the two joint axes of the link in the
533⎇⎇direction from x(i-1) to xi. The y axis completes the right
534⎇⎇handed set.
535⎇⎇We can relate between coordinate systems i+1 and i by
536⎇⎇performing a rotation, followed by two translations,
537⎇⎇followed by a rotation in the following manner:
538⎇⎇ 1). A rotation about zi of O␈-1⎇-i to align xi with
539⎇⎇ x(i+1) the common normal.
540⎇⎇ 2). A translation si along zi to locate the origin
541⎇⎇ on the common normal.
542⎇⎇ 3). A translation of ai along x(i+1) to bring the
543⎇⎇ origins into coincidence.
544⎇⎇ 4). A rotation about x(i+1) of αi to bring the z
545⎇⎇ axes together.
546⎇⎇If this represents a revolute joint then O␈-1⎇- is the joint
547⎇⎇variable. If it is a prismatic joint then s is the joint
548⎇⎇variable.
549⎇⎇If we express points in link i by a vector Ri␈-2⎇␈π_␈π_ then the
550⎇⎇relationship between coordinate systems Ri␈-2⎇␈π_␈π_ and R(i-1)␈-6⎇␈π_␈π_␈π_␈π_␈π_␈π_ may be
551⎇⎇expressed by
552⎇⎇␈→60⎇0⎇⎇R(i-1)␈-6⎇␈π_␈π_␈π_␈π_␈π_␈π_ = | Ai| * | Ri|␈←␈→60⎇-1000⎇⎇[Eq. 3.1]␈←
553⎇⎇where | Ai | is given by:
554⎇⎇␈→60⎇0⎇⎇| cos O␈-1⎇- -cos α sin O␈-1⎇- sin α cos O␈-1⎇- a cos O␈-1⎇- |␈←
555⎇⎇␈→60⎇0⎇⎇| sin O␈-1⎇- cos α cos O␈-1⎇- -sin α cos O␈-1⎇- a sin O␈-1⎇- |␈←
556⎇⎇␈→60⎇0⎇⎇| 0 sin α cos α s |␈←
557⎇⎇␈→60⎇0⎇⎇| 0 0 0 1 |␈←
558⎇⎇␈→60⎇0⎇⎇␈←␈→60⎇-1000⎇⎇[Eq. 3.2]␈←
559⎇⎇if we let link0 be the table coordinate system then we may
560⎇⎇relate from any link coordinates to R0␈-2⎇␈π_␈π_ by
561⎇⎇␈→60⎇0⎇⎇R0␈-2⎇␈π_␈π_ =|A1|*|A2|*|A3| ... |Ai|*|Ri|␈←␈→60⎇-1000⎇⎇[Eq. 3.3]␈←
562⎇⎇July 20, 1972␈→60⎇-1000⎇⎇3.1 KINEMATICS␈←
563⎇⎇␈→60⎇0⎇⎇ 16445⎇␈←␈→60⎇-1000⎇⎇␈←
564⎇⎇or
565⎇⎇␈→60⎇0⎇⎇R0␈-2⎇␈π_␈π_ = | Ti| * | Ri|␈←␈→60⎇-1000⎇⎇[Eq. 3.4]␈←
566⎇⎇where
567⎇⎇␈→60⎇0⎇⎇| Ti| = |A1|*|A2|*|A3| ... |Ai|␈←␈→60⎇-1000⎇⎇[Eq. 3.5]␈←
568⎇⎇| Ti | is the transform of the ith link of the arm and is
569⎇⎇the same as an instance transform.
570⎇⎇Based on these matrices we can obtain the differential
571⎇⎇change dR0␈-2⎇␈π_␈π_ with respect to a change of joint variable dq:
572⎇⎇␈→60⎇0⎇⎇dR0␈-2⎇␈π_␈π_ = ~␈-1⎇>␈-1⎇␈π_␈-1⎇␈↑i␈↓␈-2⎇␈↓j=1␈↑ |Uij| * dqj * |Ri|␈←␈→60⎇-1000⎇⎇[Eq. 3.6]␈←
573⎇⎇where
574⎇⎇␈→60⎇0⎇⎇|Uij| = ∂|Ti|/∂qj␈←␈→60⎇-1000⎇⎇[Eq. 3.7]␈←
575⎇⎇␈→60⎇0⎇⎇= |A1|*|A2| ... |Aj-1|*|Qj|*|Aj|*|Aj+1| ... *|Ai|␈←
576⎇⎇␈→60⎇0⎇⎇␈←␈→60⎇-1000⎇⎇[Eq. 3.8]␈←
577⎇⎇and depending on whether the joint is rotary or prismatic
578⎇⎇ | 0-1 0 0 |
579⎇⎇ Q(O␈-1⎇-) = | 1 0 0 0 |␈→60⎇-1000⎇⎇[Eq. 3.9]␈←
580⎇⎇ | 0 0 0 0 |
581⎇⎇ | 0 0 0 0 |
582⎇⎇ | 0 0 0 0 |
583⎇⎇ Q(s) = | 0 0 0 0 |␈→60⎇-1000⎇⎇[Eq. 3.10]␈←
584⎇⎇ | 0 0 0 1 |
585⎇⎇ | 0 0 0 0 |
586⎇⎇In the case of the arm that we are using, we pick the origin
587⎇⎇of coordinates at the shoulder. We have two revolute joints
588⎇⎇followed by a prismatic joint followed by three intersecting
589⎇⎇revolute joints. The only offsets are S2 = 6.0in and S6 =
590⎇⎇10.25in.
591⎇⎇July 20, 1972␈→60⎇-1000⎇⎇␈←
592⎇⎇␈→60⎇0⎇⎇ 16446⎇␈←␈→60⎇-1000⎇⎇␈←
593⎇⎇3.2 ARM SOLUTION
594⎇⎇The arm solution is that procedure which, given |T6|, the
595⎇⎇transformation of the hand, returns the six joint angles
596⎇⎇which will position the arm such that the hand has the
597⎇⎇required transformation.
598⎇⎇Because the last three joints intersect and there is only
599⎇⎇one offset, S6, we can obtain the position of the end of
600⎇⎇link 3 by subtracting a vector A␈-1⎇␈π_ the length of S6 and in the
601⎇⎇direction of the approach vector from the position vector P␈-1⎇␈π_.
602⎇⎇See Figure 3.3 to obtain W␈-1⎇␈π_.
603⎇⎇␈→60⎇0⎇⎇W␈-1⎇␈π_ = P␈-1⎇␈π_ - A␈-1⎇␈π_␈←␈→60⎇-1000⎇⎇[Eq. 3.11]␈←
604⎇⎇We can now solve for S3 the prismatic joint variable as:
605⎇⎇␈→60⎇0⎇⎇S3 = ( W␈-1⎇␈π_ . W␈-1⎇␈π_ - S2↑2 )↑(1/2)␈←␈→60⎇-1000⎇⎇[Eq. 3.12]␈←
606⎇⎇Having solved for S3, O␈-1⎇-1 is given by:
607⎇⎇␈→60⎇0⎇⎇O␈-1⎇-1 = O␈-1⎇- + O␈-1⎇|␈←␈→60⎇-1000⎇⎇[Eq. 3.13]␈←
608⎇⎇Where O␈-1⎇- and O␈-1⎇| are given by:
609⎇⎇␈→60⎇0⎇⎇Tan O␈-1⎇- = W[2] / W[1]␈←␈→60⎇-1000⎇⎇[Eq. 3.14]␈←
610⎇⎇␈→60⎇0⎇⎇Sin O␈-1⎇| = S2 / ( W[1]↑2 + W[2]↑2 )↑(1/2)␈←␈→60⎇-1000⎇⎇[Eq. 3.15]␈←
611⎇⎇See Figure 3.3
612⎇⎇O␈-1⎇-2 is then given by:
613⎇⎇␈→60⎇0⎇⎇Cos O␈-1⎇-2 = W[3] / S3␈←␈→60⎇-1000⎇⎇[Eq. 3.16]␈←
614⎇⎇See Figure 3.3.
615⎇⎇The unit vectors y3 and z3 are now calculated for link 3 and
616⎇⎇a vector:
617⎇⎇␈→60⎇0⎇⎇RR␈-2⎇␈π_␈π_ = z3␈-2⎇␈π_␈π_ x z6␈-2⎇␈π_␈π_␈←␈→60⎇-1000⎇⎇[Eq. 3.17]␈←
618⎇⎇is calculated.
619⎇⎇Then:
620⎇⎇␈→60⎇0⎇⎇O␈-1⎇-4 angle between RR␈-2⎇␈π_␈π_ and y3␈-2⎇␈π_␈π_ about z3␈-2⎇␈π_␈π_␈←␈→60⎇-1000⎇⎇[Eq. 3.18]␈←
621⎇⎇July 20, 1972␈→60⎇-1000⎇⎇3.2 SOLUTION␈←
622⎇⎇␈→60⎇0⎇⎇ 16449⎇␈←␈→60⎇-1000⎇⎇␈←
623⎇⎇␈→60⎇0⎇⎇O␈-1⎇-5 angle between z6␈-2⎇␈π_␈π_ and z3␈-2⎇␈π_␈π_ about RR␈-2⎇␈π_␈π_␈←␈→60⎇-1000⎇⎇[Eq. 3.19]␈←
624⎇⎇␈→60⎇0⎇⎇O␈-1⎇-6 angle between y6␈-2⎇␈π_␈π_ and RR␈-2⎇␈π_␈π_ about z6␈-2⎇␈π_␈π_␈←␈→60⎇-1000⎇⎇[Eq. 3.20]␈←
625⎇⎇See Figure 3.4
626⎇⎇All joints with the exception of joint 6 have only a partial
627⎇⎇range of motion. As the solution for each joint is obtained,
628⎇⎇it must be checked to see that it is within the range of
629⎇⎇motion of that joint.
630⎇⎇3.3 DIFFERENTIAL SOLUTION
631⎇⎇Given an arm solution it is often necessary to compute the
632⎇⎇differential change in joint angles in order to make a small
633⎇⎇change in position while maintaining the present orientation
634⎇⎇of the hand.
635⎇⎇We already have expressions for the differential change of
636⎇⎇| Ti| Equation 3.7 in terms of dqj the differential change
637⎇⎇in the j joint angles.
638⎇⎇If we evaluate the six | U6j| matrices (Equation 3.7) we
639⎇⎇will have the differential change in the 16 elements of the
640⎇⎇hand matrix | T6 | for each of the six joints. This
641⎇⎇corresponds to 16 equations in 6 unknowns. We must pick 6
642⎇⎇of the 16 equations and then solve for the required change
643⎇⎇in the qj.
644⎇⎇From | U6j | we will pick the first three elements of the
645⎇⎇right hand column as these correspond to dx, dy, dz. We then
646⎇⎇pick the two smallest elements of column 3, the approach
647⎇⎇vector, to constrain its direction. Finally we pick one
648⎇⎇additional element from column 2, the orientation vector, in
649⎇⎇order to constrain rotation about the approach vector.
650⎇⎇The six equations:
651⎇⎇| dx | | U61 U62 U63 U64 U65 U66 | | dq |
652⎇⎇| | | 14 14 14 14 14 14 | | 1|
653⎇⎇| | | | | |
654⎇⎇| dy | | U61 U62 U63 U64 U65 U66 | | dq |
655⎇⎇| | | 24 24 24 24 24 24 | | 2|
656⎇⎇| | | | | |
657⎇⎇| dz | = | U61 U62 U63 U64 U65 U66 | * | dq |
658⎇⎇| | | 34 34 34 34 34 34 | | 3|
659⎇⎇July 20, 1972␈→60⎇-1000⎇⎇3.3 DIFFERENTIAL␈←
660⎇⎇␈→60⎇0⎇⎇ 16452⎇␈←␈→60⎇-1000⎇⎇␈←
661⎇⎇| | | | | |
662⎇⎇| 0 | | U61 U62 U63 U64 U65 U66 | | dq |
663⎇⎇| | | u3 u3 u3 u3 u3 u3 | | 3|
664⎇⎇| | | | | |
665⎇⎇| 0 | | U61 U62 U63 U64 U65 U66 | | dq |
666⎇⎇| | | v3 v3 v3 v3 v3 v3 | | 4|
667⎇⎇| | | | | |
668⎇⎇| 0 | | U61 U62 U63 U64 U65 U66 | | dq |
669⎇⎇| | | w2 w2 w2 w2 w2 w2 | | 6|
670⎇⎇␈→60⎇-1000⎇⎇[Eq. 3.21]␈←
671⎇⎇are then solved to give the six dqj, the differential change
672⎇⎇in joint angle.
673⎇⎇3.4 DYNAMICS
674⎇⎇From the kinematic arm model we can also develop the dynamic
675⎇⎇model [Uicker]. We will derive the Lagrangian [Simon] for
676⎇⎇the arm in a gravitational force field, and obtain the
677⎇⎇equations relating acceleration to joint torque, including
678⎇⎇the static torques necessary to overcome the effects of
679⎇⎇gravity. These results will be used in the section which
680⎇⎇relates to servoing the arm.
681⎇⎇If the Lagrangian L is defined as
682⎇⎇␈→60⎇0⎇⎇L = K - P␈←␈→60⎇-1000⎇⎇[Eq. 3.22]␈←
683⎇⎇ where:
684⎇⎇ K is the Kinetic Energy of the system in terms of
685⎇⎇ joint variables
686⎇⎇ P is the potential energy in terms of joint
687⎇⎇ variables
688⎇⎇July 20, 1972␈→60⎇-1000⎇⎇3.4 DYNAMICS␈←
689⎇⎇␈→60⎇0⎇⎇ 16454⎇␈←␈→60⎇-1000⎇⎇␈←
690⎇⎇ The joint variables "q" are either "O␈-1⎇-" or "s"
691⎇⎇ depending upon whether the joint is revolute or
692⎇⎇ prismatic.
693⎇⎇The equations of motion are given by:
694⎇⎇␈→60⎇0⎇⎇d/dt ( ∂L/∂q␈-1⎇␈↑.␈↓j ) - ∂L/∂qj = Fj␈←␈→60⎇-1000⎇⎇[Eq. 3.23]␈←
695⎇⎇␈→60⎇0⎇⎇for j = 1,2, ... n␈←
696⎇⎇where Fj is the force on joint j.
697⎇⎇From Equation 3.6 we can obtain the velocity of any point
698⎇⎇as:
699⎇⎇␈→60⎇0⎇⎇dR0␈-2⎇␈π_␈π_/dt = | Vi | * | Ri |␈←␈→60⎇-1000⎇⎇[Eq. 3.24]␈←
700⎇⎇where:
701⎇⎇␈→60⎇0⎇⎇| Vi | = ~␈-1⎇>␈-1⎇␈π_␈-1⎇␈↑i␈↓␈-2⎇␈↓j=1␈↑( | Uij | * dqj/dt )␈←␈→60⎇-1000⎇⎇[Eq. 3.25]␈←
702⎇⎇We may now express the kinetic energy of a link as follows.
703⎇⎇Consider a particle of mass dm on link i at Ri␈-2⎇␈π_␈π_ then the
704⎇⎇kinetic energy is:
705⎇⎇␈→60⎇0⎇⎇dHi = 1/2 ( R␈-1⎇␈↑.␈↓i␈-2⎇␈π_␈π_ . R␈-1⎇␈↑.␈↓i␈-2⎇␈π_␈π_ ) dm␈←␈→60⎇-1000⎇⎇[Eq. 3.26]␈←
706⎇⎇or:
707⎇⎇␈→60⎇0⎇⎇dHi = 1/2 Trace(| Vi|*| Ri|*| Ri|␈↑T␈↓*| Vi|␈↑T␈↓)*dm␈←
708⎇⎇␈→60⎇0⎇⎇␈←␈→60⎇-1000⎇⎇[Eq. 3.27]␈←
709⎇⎇The total kinetic energy for the link can be found by
710⎇⎇integrating over the mass of the link.
711⎇⎇␈→60⎇0⎇⎇dHi = 1/2 Trace[|Vi|*(␈π~␈-3⎇␈↓link␈↑|Ri|*|Ri|␈↑T␈↓*dm)*|Vi|␈↑T␈↓]␈←
712⎇⎇␈→60⎇0⎇⎇␈←␈→60⎇-1000⎇⎇[Eq. 3.28]␈←
713⎇⎇July 20, 1972␈→60⎇-1000⎇⎇3.4 DYNAMICS␈←
714⎇⎇␈→60⎇0⎇⎇ 16455⎇␈←␈→60⎇-1000⎇⎇␈←
715⎇⎇to obtain:
716⎇⎇Hi = mi*
717⎇⎇␈→60⎇0⎇⎇| 2 2 2 2 2 ␈π_ |␈←
718⎇⎇␈→60⎇0⎇⎇|1/2(-k +k +k ) k k x |␈←
719⎇⎇␈→60⎇0⎇⎇| i11 i22 i33 i12 i13 i|␈←
720⎇⎇␈→60⎇0⎇⎇| |␈←
721⎇⎇␈→60⎇0⎇⎇| 2 2 2 2 2 ␈π_ |␈←
722⎇⎇␈→60⎇0⎇⎇| k k -k +k k y |␈←
723⎇⎇␈→60⎇0⎇⎇| i12 i11 i22 i33 i23 i|␈←
724⎇⎇␈→60⎇0⎇⎇| |␈←
725⎇⎇␈→60⎇0⎇⎇| 2 2 2 2 2 ␈π_ |␈←
726⎇⎇␈→60⎇0⎇⎇| k k k +k -k z |␈←
727⎇⎇␈→60⎇0⎇⎇| i13 i23 i11 i22 i33 i|␈←
728⎇⎇␈→60⎇0⎇⎇| |␈←
729⎇⎇␈→60⎇0⎇⎇| ␈π_ ␈π_ ␈π_ |␈←
730⎇⎇␈→60⎇0⎇⎇| x y z 1 |␈←
731⎇⎇␈→60⎇0⎇⎇| i i i |␈←
732⎇⎇␈→60⎇0⎇⎇␈←␈→60⎇-1000⎇⎇[Eq. 3.29]␈←
733⎇⎇ where:k␈↓ijk␈↑ is the radius of gyration of link i about
734⎇⎇ the j,k axes. x␈-1⎇␈↑_␈↓i, y␈-1⎇␈↑_␈↓i, z␈-1⎇␈↑_␈↓i is the center of
735⎇⎇ mass of link i. mi is the mass of link i.
736⎇⎇The kinetic energy of the entire system is
737⎇⎇␈→60⎇0⎇⎇K = 1/2 ~␈-1⎇>␈-1⎇␈π_␈-1⎇␈↑n␈↓␈-2⎇␈↓i=1␈↑ Trace(| Vi |*| Hi |*| Vi |␈↑T␈↓)␈←␈→60⎇-1000⎇⎇[Eq. 3.30]␈←
738⎇⎇The potential energy of the system due to gravity in the
739⎇⎇negative z direction, is expressed by:
740⎇⎇␈→60⎇0⎇⎇P = - ~␈-1⎇>␈-1⎇␈π_␈-1⎇␈↑n␈↓␈-2⎇␈↓i=1␈↑ mi * | G | * | Ti | * | R␈-1⎇␈↑_␈↓i |␈←␈→60⎇-1000⎇⎇[Eq. 3.31]␈←
741⎇⎇where | G | = | 0 0 g 0 |␈→60⎇-1000⎇⎇[Eq. 3.32]␈←
742⎇⎇and g is the acceleration due to gravity.
743⎇⎇July 20, 1972␈→60⎇-1000⎇⎇3.4 DYNAMICS␈←
744⎇⎇␈→60⎇0⎇⎇ 16456⎇␈←␈→60⎇-1000⎇⎇␈←
745⎇⎇Substituting for K from Equation 3.30 and for P from
746⎇⎇Equation 3.31 into Equation 3.22 and then differentiating
747⎇⎇according to Equation 3.23 we obtain
748⎇⎇ Fi = ~␈-1⎇>␈-1⎇␈π_␈-1⎇␈↑n␈↓␈-2⎇␈↓j=i␈↑[~␈-1⎇>␈-1⎇␈π_␈-1⎇␈↑j␈↓␈-2⎇␈↓k=1␈↑Trace(| Ujk|*| Hj|*| Uji|␈↑T␈↓*qk␈-2⎇␈↑..␈↓)
749⎇⎇ +~␈-1⎇>␈-1⎇␈π_␈-1⎇␈↑j␈↓␈-2⎇␈↓k=1␈↑ ~␈-1⎇>␈-1⎇␈π_␈-1⎇␈↑j␈↓␈-2⎇␈↓l=1␈↑Trace(| Ujkl|*| Hj|*| Uji|␈↑T␈↓*q␈-1⎇␈↑.␈↓j*q␈-1⎇␈↑.␈↓k)
750⎇⎇ -mj*| G |*| Uji|*| R␈-1⎇␈↑_␈↓j]␈→60⎇-1000⎇⎇[Eq. 3.33]␈←
751⎇⎇the equation relating acceleration to force, from which we
752⎇⎇will infer the effective link inertia and gravity loading
753⎇⎇torque Tg in Subsection 330⎇.
754⎇⎇3.5 FORCE AND MOMENTS
755⎇⎇Given a force F␈-1⎇␈π_ that acts through the origin of the hand
756⎇⎇coordinate system and a moment M, we wish to find the joint
757⎇⎇reaction torques.
758⎇⎇We represent a force in the n'th coordinate system as:
759⎇⎇␈→60⎇0⎇⎇Fn = | Fn[x] Fn[y] Fn[z] 0 |␈←␈→60⎇-1000⎇⎇[Eq. 3.34]␈←
760⎇⎇and a moment similarly:
761⎇⎇␈→60⎇0⎇⎇Mn = | Mn[x] Mn[y] Mn[z] 0 |␈←␈→60⎇-1000⎇⎇[Eq. 3.35]␈←
762⎇⎇We first transform the force F and the moment M into the
763⎇⎇hand coordinate system by:
764⎇⎇␈→60⎇0⎇⎇| F6 | = | T6 |␈↑-1␈↓*| F |␈←␈→60⎇-1000⎇⎇[Eq. 3.36]␈←
765⎇⎇␈→60⎇0⎇⎇| M6 | = | T6 |␈↑-1␈↓*| M |␈←␈→60⎇-1000⎇⎇[Eq. 3.37]␈←
766⎇⎇July 20, 1972␈→60⎇-1000⎇⎇3.5 FORCE␈←
767⎇⎇␈→60⎇0⎇⎇ 16458⎇␈←␈→60⎇-1000⎇⎇␈←
768⎇⎇We then proceed to transform the force and moment back
769⎇⎇through the links as follows. (see Figure 3.5)
770⎇⎇␈→60⎇0⎇⎇| F(n-1) | = | An | * | Fn |␈←␈→60⎇-1000⎇⎇[Eq. 3.38]␈←
771⎇⎇and the moment:
772⎇⎇␈→60⎇0⎇⎇| M(n-1) | = | An | * | Mn |␈←␈→60⎇-1000⎇⎇[Eq. 3.39]␈←
773⎇⎇However the force F(n-1) does not act through the origin of
774⎇⎇the (n-1)th coordinate system (see Figure 3.5) and we have
775⎇⎇an additional moment given by:
776⎇⎇␈→60⎇0⎇⎇P␈-1⎇␈π_ x F␈-1⎇␈π_␈←␈→60⎇-1000⎇⎇[Eq. 3.40]␈←
777⎇⎇where:
778⎇⎇␈→60⎇0⎇⎇␈_F(n-1)␈≡ is | F(n-1)[x] F(n-1)[y] F(n-1)[z] 1 |␈←
779⎇⎇␈→60⎇0⎇⎇␈←␈→60⎇-1000⎇⎇[Eq. 3.41]␈←
780⎇⎇and P␈-1⎇␈π_ is the right hand column of | An |
781⎇⎇␈→60⎇0⎇⎇P␈-1⎇␈π_ = | An[1,4] An[2,4] An[3,4] 1 |␈←␈→60⎇-1000⎇⎇[Eq. 3.42]␈←
782⎇⎇the total moment is then:
783⎇⎇␈→60⎇0⎇⎇| M(n-1) | = | An | * | Mn | + P␈-1⎇␈π_ x ␈_F(n-1)␈≡␈←␈→60⎇-1000⎇⎇[Eq. 3.43]␈←
784⎇⎇If the (n-1)th joint is revolute then the reaction torque is
785⎇⎇M(n-1)␈↓z␈↑ if it is prismatic then the force is F(n-1)␈↓z␈↑.
786⎇⎇The six reaction torques are known as the equivalent arm
787⎇⎇torque.
788⎇⎇When the hand is to exert a force the equivalent arm torque
789⎇⎇is calculated and added to the gravity torque for each
790⎇⎇joint. If each joint is run at these torques then the hand
791⎇⎇will exert the required force.
792⎇⎇July 20, 1972␈→60⎇-1000⎇⎇␈←
793⎇⎇␈→60⎇0⎇⎇ 16460⎇␈←␈→60⎇-1000⎇⎇␈←
794⎇⎇␈→60⎇0⎇⎇4.0 MOVE INSTANCE␈←
795⎇⎇The procedure MOVE␈π_INSTANCE is called when it is desired to
796⎇⎇have the arm move an object. MOVE␈π_INSTANCE is required to
797⎇⎇find two suitable hand positions such that if the arm
798⎇⎇assumes the first position it will be in a position to grasp
799⎇⎇the object located by | Ti |, and then if it moves to the
800⎇⎇second arm position and releases the object the required
801⎇⎇move will have been accomplished, and the object will be at
802⎇⎇| Tf |. The suffixes "i" and "f" refer to the initial and
803⎇⎇final positions.
804⎇⎇4.1 RANGE OF SOLUTION
805⎇⎇In manipulating objects we need to find not only a single
806⎇⎇solution but a range of solutions. Given a position and an
807⎇⎇orientation vector, the program should compute the range of
808⎇⎇approach angle throughout which the arm can reach the object
809⎇⎇to perform the desired manipulation.
810⎇⎇The approach angle is the angle measured between the
811⎇⎇approach vector A␈-1⎇␈π_ and the reference approach vector RA␈-2⎇␈π_␈π_ which
812⎇⎇is given by:
813⎇⎇␈→60⎇0⎇⎇RA␈-2⎇␈π_␈π_ ← z␈-1⎇␈π_ x O␈-1⎇␈π_␈←␈→60⎇-1000⎇⎇[Eq. 4.1]␈←
814⎇⎇where:
815⎇⎇␈→60⎇0⎇⎇z␈-1⎇␈π_ = | 0 0 1 1 |␈←␈→60⎇-1000⎇⎇[Eq. 4.2]␈←
816⎇⎇It is doubtful that this problem can be solved analytically.
817⎇⎇Heuristics are used to find an approach angle at which an
818⎇⎇arm solution will exist, if one exists at all, and a further
819⎇⎇test is made to determine if solutions exist for all
820⎇⎇approach angles. If a solution exists, but not for the full
821⎇⎇range, a binary search is conducted, using the arm solution
822⎇⎇procedure as a test, to define the limits of the range. This
823⎇⎇is quite fast as the limits of the range are only needed to
824⎇⎇within ␈π-5 degrees, and the arm solution is very simple.
825⎇⎇The range of approach angle is that through which the arm
826⎇⎇can reach the object for a given position and orientation
827⎇⎇vector see (Figure 4.1).
828⎇⎇July 20, 1972␈→60⎇-1000⎇⎇4.1 RANGE␈←
829⎇⎇␈→60⎇0⎇⎇ 16464⎇␈←␈→60⎇-1000⎇⎇␈←
830⎇⎇We then consider the support plane and obtain a range of
831⎇⎇approach angle for the same position and orientation vector,
832⎇⎇limited by the support plane. This is solved for
833⎇⎇approximately, assuming the worst case condition of link 5
834⎇⎇See Figure 4.2.
835⎇⎇This range of approach angle is then intersected with the
836⎇⎇possible range of approach angle defined by the arm to give
837⎇⎇a range of approach angle throughout which the arm can reach
838⎇⎇the object considering both the arm and the support.
839⎇⎇This process is repeated for the support post of the arm. A
840⎇⎇final intersection is made to keep the hand from
841⎇⎇intersecting the post, and the limiting range of approach
842⎇⎇angle is obtained.
843⎇⎇4.2 MOVE INSTANCE
844⎇⎇The first action of MOVE␈π_INSTANCE is to select those
845⎇⎇orientation vectors of the prototype ( see section 2.2)
846⎇⎇which can be used to grasp the object. To do this we
847⎇⎇transform a gravity vector back to the prototype using the
848⎇⎇inverse transform and determine through which face it
849⎇⎇passes. This is then the support face. Any orientation
850⎇⎇vector, (Equation 2.9) which has a contact point (Equation
851⎇⎇2.10) on this face or on any edge or vertex of this face is
852⎇⎇then discarded; the remaining orientation vectors are marked
853⎇⎇as possible. This procedure is performed for the object in
854⎇⎇its initial position and again in its final position. If
855⎇⎇the action is to be accomplished in one move then the set of
856⎇⎇available orientation vectors that can be used is the
857⎇⎇intersection of the two sets of orientation vectors for the
858⎇⎇initial and final positions. These orientation vectors are
859⎇⎇then ordered by length; the shortest, representing face -
860⎇⎇face contacts, are considered first, as this is the most
861⎇⎇stable way that an object may be picked-up.
862⎇⎇For each of the orientation vectors so ordered the range of
863⎇⎇approach is calculated at both the initial and final
864⎇⎇positions. By transforming the reference approach vector
865⎇⎇(Equation 4.1) at the initial position to the final position
866⎇⎇by:
867⎇⎇␈→60⎇0⎇⎇RAi␈-3⎇␈π_␈π_␈π_' ← | Tf | * | Ti |␈↑-1␈↓ * | RAi |␈←␈→60⎇-1000⎇⎇[Eq. 4.3]␈←
868⎇⎇July 20, 1972␈→60⎇-1000⎇⎇4.2 MOVE␈←
869⎇⎇␈→60⎇0⎇⎇ 16467⎇␈←␈→60⎇-1000⎇⎇␈←
870⎇⎇the shift S, between approach ranges may then be calculated
871⎇⎇as:
872⎇⎇␈→60⎇0⎇⎇S ← the angle between RAi␈-3⎇␈π_␈π_␈π_' and RAf␈-3⎇␈π_␈π_␈π_ about Of␈-2⎇␈π_␈π_␈←
873⎇⎇␈→60⎇0⎇⎇␈←␈→60⎇-1000⎇⎇[Eq. 4.4]␈←
874⎇⎇After applying the shift S to the initial range the two
875⎇⎇ranges of approach are then intersected. If the
876⎇⎇intersection is not empty then the solution has been found.
877⎇⎇An approach vector A␈-1⎇␈π_ is picked within the range of
878⎇⎇intersection, vertically down if this is within the range,
879⎇⎇and two arm solutions are obtained, one at the initial
880⎇⎇position and one at the final position, such that the shift
881⎇⎇between approach vectors is maintained.
882⎇⎇If there is no intersection between the first two ranges,
883⎇⎇the next orientation vector in the intersection of the
884⎇⎇available set of orientation vectors is tried.
885⎇⎇When the set of orientation vectors is empty at either the
886⎇⎇initial or final positions, then there is no way that the
887⎇⎇move can be accomplished as the arm cannot reach the object.
888⎇⎇If the intersection of the set of orientation vectors was
889⎇⎇empty, or if after evaluating the ranges for all the
890⎇⎇orientation vectors the intersection of the ranges was empty
891⎇⎇then an intermediate position would be tried.
892⎇⎇An attempt is made to move the object from its initial
893⎇⎇position to an intermediate position and then from the
894⎇⎇intermediate position to the final position. The calling
895⎇⎇program specifies whether an intermediate position is to be
896⎇⎇tried and if so where. A clear space in front of the arm
897⎇⎇will give the greatest range of approach, however choosing
898⎇⎇the initial of final position as the intermediate position
899⎇⎇ensures that it is clear, and the strategy subsystem does
900⎇⎇not have to find a clear space.
901⎇⎇Even when the initial position is used as the intermediate
902⎇⎇position, the move is usually accomplished, as it is only
903⎇⎇the position and not the orientation that is specified. On
904⎇⎇most occasions when the arm is unable to carry out a task in
905⎇⎇one move, it is because the object must be turned over.It is
906⎇⎇then necessary for the arm to turn the object part-way over,
907⎇⎇put it down, and pick it up again to complete the move.
908⎇⎇Given the new intermediate position, a set of possible new
909⎇⎇July 20, 1972␈→60⎇-1000⎇⎇4.2 MOVE␈←
910⎇⎇␈→60⎇0⎇⎇ 16468⎇␈←␈→60⎇-1000⎇⎇␈←
911⎇⎇support faces is determined as the intersection of the set
912⎇⎇of faces which are neighbors to the original support face
913⎇⎇and to the final support face. A transformation is
914⎇⎇constructed for this intermediate position to bring the new
915⎇⎇support face parallel to the original support surface. The
916⎇⎇object is then turned about a normal axis to the support
917⎇⎇such that its principal axes are perpendicular to the
918⎇⎇shoulder of the arm, as this gives the maximum freedom of
919⎇⎇approach range.
920⎇⎇The height of the center of the object is adjusted if the
921⎇⎇distance from the center of mass to the original support
922⎇⎇face is different from the present distance.
923⎇⎇The process is now repeated to find a common approach. First
924⎇⎇a move solution to the intermediate position is computed and
925⎇⎇then, if successful, a move solution from the intermediate
926⎇⎇position to the final is attempted . Ranges are saved during
927⎇⎇the "initial - final" move attempt as they are needed in the
928⎇⎇"initial - intermediate", "intermediate - final" move
929⎇⎇evaluations.
930⎇⎇By this procedure it is possible to make any re-positionings
931⎇⎇and re-orientations, even when the goal must be accomplished
932⎇⎇in two moves.
933⎇⎇About 4 seconds are required to compute the arm positions
934⎇⎇when two moves must be used.
935⎇⎇July 20, 1972␈→60⎇-1000⎇⎇␈←
936⎇⎇␈→60⎇0⎇⎇ 16469⎇␈←␈→60⎇-1000⎇⎇␈←
937⎇⎇␈→60⎇0⎇⎇5.0 TRAJECTORIES␈←
938⎇⎇5.1 GENERAL CONSIDERATIONS
939⎇⎇In moving the arm we have two positions, the initial and
940⎇⎇final. The discussion until now has emphasized these
941⎇⎇positions and their determination (Subsection 4.2). In this
942⎇⎇section we will describe the move in detail.
943⎇⎇The simplest solution is to move the joints independently
944⎇⎇from their initial position to their final position, using a
945⎇⎇simple velocity-damped position servo. Consider the
946⎇⎇situation shown in figure Figure 5.1. The motion is mostly
947⎇⎇in joint 5; if all the joints were moved to their final
948⎇⎇positions then the hand would move through the support.
949⎇⎇What is needed is to lift the arm up and down as joint 5 is
950⎇⎇moved, in order to clear the support. Two points should be
951⎇⎇made: 1)some joints cannot move directly from their initial
952⎇⎇to their final position; 2) the motion of all joints should
953⎇⎇be coordinated.
954⎇⎇When the arm starts to move, it is normally working with
955⎇⎇respect to some surface, for instance, picking up a block
956⎇⎇from a table. As it starts to move the motion of the hand
957⎇⎇should be directly away from the surface.
958⎇⎇If we were to specify a position on a normal to the surface
959⎇⎇out from the initial position, and then to require that the
960⎇⎇hand pass through this position, we would achieve the
961⎇⎇correct departure motion. If we could further specify the
962⎇⎇time required to reach this position, we could control the
963⎇⎇speed at which the block was to be lifted.
964⎇⎇To estimate how far this position should be from the
965⎇⎇surface, consider Figure 5.2, as this represents the worst
966⎇⎇case. If the hand were lifted by the maximum surface
967⎇⎇penetration, approximately 25% the length of the last link,
968⎇⎇the collision would have been avoided.
969⎇⎇For such an initial move, the differential change of joint
970⎇⎇angles is calculated (Subsection 3.3) for a move in the
971⎇⎇direction of the outward pointing normal of 3 inches. A
972⎇⎇time based on a fairly low arm force is then calculated. The
973⎇⎇same set of requirements exists in the case of the final
974⎇⎇July 20, 1972␈→60⎇-1000⎇⎇5.1 GENERAL␈←
975⎇⎇␈→60⎇0⎇⎇ 16474⎇␈←␈→60⎇-1000⎇⎇␈←
976⎇⎇position. Here we wish once again to approach the surface in
977⎇⎇the direction of the normal. The time is increased to
978⎇⎇ensure a smooth setdown.
979⎇⎇We now have 4 positions: initial,liftoff,letdown, and final.
980⎇⎇At present the arm works in a clear space and the "collision
981⎇⎇avoider" has not been implemented. If it were we might have
982⎇⎇additional points through which we would require the arm to
983⎇⎇pass. If we were to servo the arm from one position to the
984⎇⎇next we would not collide with the support (see Figure 5.3).
985⎇⎇We would, however, like the arm to start and end its motion
986⎇⎇with zero velocity and acceleration, Further there is no
987⎇⎇need to stop the arm at all the intermediate positions. All
988⎇⎇that we require is that the joints of the arm pass through
989⎇⎇the trajectory points corresponding to these intermediate
990⎇⎇positions at the same time.
991⎇⎇By estimating the time for each joint to move through the
992⎇⎇segments of its motion and specifying the same maximum
993⎇⎇segment time for all joints, we can determine a polynomial
994⎇⎇for each joint, which passes through all the points and has
995⎇⎇zero initial and final velocity and acceleration.
996⎇⎇Although such trajectories satisfy our conditions, they
997⎇⎇often have extrema between the initial and final points. We
998⎇⎇must evaluate the joint variable at each extremum to check
999⎇⎇that it has not exceeded the working range of the joint.
1000⎇⎇As the extrema are difficult to evaluate for high order
1001⎇⎇polynomials, we use a different approach. We specify three
1002⎇⎇polynomials, one for the trajectory from the initial point
1003⎇⎇to the liftoff point. A second trajectory from the liftoff
1004⎇⎇to the setdown point, and a third from the setdown to the
1005⎇⎇final point. We specify that velocity and acceleration
1006⎇⎇should be zero at the initial and final points and that they
1007⎇⎇should be continuous at the intermediate points. This
1008⎇⎇sequence of polynomials satisfies our conditions for a
1009⎇⎇trajectory, and has extrema which are easily evaluated.
1010⎇⎇If a joint exceeds its working range at an extremum, then
1011⎇⎇the trajectory segment in which it occurs is split in two,
1012⎇⎇and a new intermediate point equal to the joint range limit
1013⎇⎇is specified at the break. (See Figure 5.4).
1014⎇⎇If an extremum occurs in the initial or final segments,
1015⎇⎇which corresponds to an overshoot of the initial or final
1016⎇⎇July 20, 1972␈→60⎇-1000⎇⎇5.1 GENERAL␈←
1017⎇⎇␈→60⎇0⎇⎇ 16478⎇␈←␈→60⎇-1000⎇⎇␈←
1018⎇⎇position (see Figure 5.5), then the liftoff point is added
1019⎇⎇as a new intermediate point through which the trajectory
1020⎇⎇should pass at a time after liftoff equal to the time
1021⎇⎇interval between the extremum and the liftoff (see Figure
1022⎇⎇5.5).
1023⎇⎇We have another type of trajectory that we wish to be able
1024⎇⎇to compute; that is to move the arm along a well defined
1025⎇⎇space curve. Here we obtain a sequence of joint angles at
1026⎇⎇equal time intervals and such that the velocity along the
1027⎇⎇space curve is controlled by relating distance along the
1028⎇⎇curve to time between points (See primitive DRAW Subsection
1029⎇⎇ 397⎇). Here we do not have any difficulties with extrema,
1030⎇⎇as was the case in the previous problem, as the space curve
1031⎇⎇is well defined. This type of curve leads to a trajectory
1032⎇⎇with many points, currently 8. If we were to use a single
1033⎇⎇polynomial it would need to be 10th order, for this reason
1034⎇⎇here also the sequence of low order polynomials is
1035⎇⎇preferred.
1036⎇⎇5.2 POLYNOMIALS
1037⎇⎇For each trajectory segment we have position, velocity and
1038⎇⎇acceleration constraints at each end. Except at the
1039⎇⎇beginning and end of the trajectory the velocity and
1040⎇⎇acceleration constraints are continuity constraints. There
1041⎇⎇are only four constraints on the intermediate trajectory
1042⎇⎇segments and five constraints at the ends. Thus for the
1043⎇⎇first and last trajectory segments a fourth order polynomial
1044⎇⎇will suffice and for the intermediate trajectory segments a
1045⎇⎇third order polynomial will be needed.
1046⎇⎇Consider a trajectory segment described by:
1047⎇⎇␈→60⎇0⎇⎇O␈-1⎇- = Ai4*t␈↑4␈↓ + Ai3*t␈↑3␈↓ + Ai2*t␈↑2␈↓ + Ai1*t +Ai0␈←␈→60⎇-1000⎇⎇[Eq. 5.1]␈←
1048⎇⎇where
1049⎇⎇␈→60⎇0⎇⎇t = t/T1␈←␈→60⎇-1000⎇⎇[Eq. 5.2]␈←
1050⎇⎇where Aij is the coefficient of the jth. power of the ith.
1051⎇⎇trajectory segment. and time is normalized to unity at the
1052⎇⎇end of the trajectory segment of duration Ti.
1053⎇⎇July 20, 1972␈→60⎇-1000⎇⎇5.2 POLYNOMIALS␈←
1054⎇⎇␈→60⎇0⎇⎇ 16480⎇␈←␈→60⎇-1000⎇⎇␈←
1055⎇⎇For the first trajectory segment at time t=0:
1056⎇⎇␈→60⎇0⎇⎇O␈-1⎇-0 = A10␈←␈→60⎇-1000⎇⎇[Eq. 5.3]␈←
1057⎇⎇␈→60⎇0⎇⎇O␈-1⎇-␈-1⎇␈↑.␈↓0 = 0 =A11␈←␈→60⎇-1000⎇⎇[Eq. 5.4]␈←
1058⎇⎇␈→60⎇0⎇⎇O␈-1⎇-0␈-2⎇␈↑..␈↓ = 0 =A12␈←␈→60⎇-1000⎇⎇[Eq. 5.5]␈←
1059⎇⎇and at time t=1:
1060⎇⎇␈→60⎇0⎇⎇O␈-1⎇-1 - O␈-1⎇-0 = ~␈-1⎇VO␈-1⎇-1 = A14 + A13␈←␈→60⎇-1000⎇⎇[Eq. 5.6]␈←
1061⎇⎇␈→60⎇0⎇⎇T1*O␈-1⎇-␈-1⎇␈↑.␈↓1 = 4*A14 + 3*A13␈←␈→60⎇-1000⎇⎇[Eq. 5.7]␈←
1062⎇⎇␈→60⎇0⎇⎇T1␈↑2␈↓*O␈-1⎇-1␈-2⎇␈↑..␈↓ = 12*A14 + 6*A13␈←␈→60⎇-1000⎇⎇[Eq. 5.8]␈←
1063⎇⎇for the last trajectory segment we substitute:
1064⎇⎇␈→60⎇0⎇⎇t' = t - 1␈←␈→60⎇-1000⎇⎇[Eq. 5.9]␈←
1065⎇⎇and at time t'=0:
1066⎇⎇␈→60⎇0⎇⎇O␈-1⎇-N = AN0␈←␈→60⎇-1000⎇⎇[Eq. 5.10]␈←
1067⎇⎇␈→60⎇0⎇⎇O␈-1⎇-␈-1⎇␈↑.␈↓n = 0 =An1␈←␈→60⎇-1000⎇⎇[Eq. 5.11]␈←
1068⎇⎇␈→60⎇0⎇⎇O␈-1⎇-n␈-2⎇␈↑..␈↓ = 0 =An2␈←␈→60⎇-1000⎇⎇[Eq. 5.12]␈←
1069⎇⎇and at time t'=-1:
1070⎇⎇␈→60⎇0⎇⎇O␈-1⎇-n - O␈-1⎇-(n-1) = ~␈-1⎇VO␈-1⎇-n = -An4 + An3␈←␈→60⎇-1000⎇⎇[Eq. 5.13]␈←
1071⎇⎇␈→60⎇0⎇⎇Tn*O␈-1⎇-␈-1⎇␈↑.␈↓(n-1) = -4*An4 + 3*An3␈←␈→60⎇-1000⎇⎇[Eq. 5.14]␈←
1072⎇⎇␈→60⎇0⎇⎇Tn␈↑2␈↓*O␈-1⎇-(n-1)␈-6⎇␈↑.. ␈↓ = 12*An4 - 6*An3␈←␈→60⎇-1000⎇⎇[Eq. 5.15]␈←
1073⎇⎇For the general ith. trajectory segment we have
1074⎇⎇␈→60⎇0⎇⎇0 = Ai4␈←␈→60⎇-1000⎇⎇[Eq. 5.16]␈←
1075⎇⎇␈→60⎇0⎇⎇O␈-1⎇-(i-1) = Ai0␈←␈→60⎇-1000⎇⎇[Eq. 5.17]␈←
1076⎇⎇␈→60⎇0⎇⎇O␈-1⎇-␈-1⎇␈↑.␈↓(i-1) = Ai1␈←␈→60⎇-1000⎇⎇[Eq. 5.18]␈←
1077⎇⎇␈→60⎇0⎇⎇O␈-1⎇-i␈-2⎇␈↑..␈↓(-1) = Ai2␈←␈→60⎇-1000⎇⎇[Eq. 5.19]␈←
1078⎇⎇␈→60⎇0⎇⎇O␈-1⎇-i - O␈-1⎇-(i-1) = ~␈-1⎇VO␈-1⎇-i = Ai3 + Ai2 + Ai1␈←␈→60⎇-1000⎇⎇[Eq. 5.20]␈←
1079⎇⎇July 20, 1972␈→60⎇-1000⎇⎇5.2 POLYNOMIALS␈←
1080⎇⎇␈→60⎇0⎇⎇ 16481⎇␈←␈→60⎇-1000⎇⎇␈←
1081⎇⎇␈→60⎇0⎇⎇O␈-1⎇-␈-1⎇␈↑.␈↓i = 3*Ai3 + 2*Ai2 +Ai1␈←␈→60⎇-1000⎇⎇[Eq. 5.21]␈←
1082⎇⎇␈→60⎇0⎇⎇O␈-1⎇-i␈-2⎇␈↑..␈↓ = 6*Ai3 + 2*Ai1␈←␈→60⎇-1000⎇⎇[Eq. 5.22]␈←
1083⎇⎇Equations 5.3, 5.4, 5.5, 5.10, 5.11, 5.12, and Equations
1084⎇⎇5.17 specify coefficients directly. The remaining
1085⎇⎇coefficients may be solved in the form:
1086⎇⎇|COEFF| =
1087⎇⎇␈→60⎇0⎇⎇| |␈←
1088⎇⎇␈→60⎇0⎇⎇| 1 1 |␈←
1089⎇⎇␈→60⎇0⎇⎇| ␈π_␈π_␈π_␈π_␈π_␈π_␈π_␈π_␈π_␈π_␈π_␈π_␈π_␈π_␈π_␈π_␈π_␈π_ |␈←
1090⎇⎇␈→60⎇0⎇⎇|3/T1 4/T1 |-1/T2 | |␈←
1091⎇⎇␈→60⎇0⎇⎇| 2 2| 2 | |␈←
1092⎇⎇␈→60⎇0⎇⎇|6/T1 12/T1 | 0 -2/T2 | |␈←
1093⎇⎇␈→60⎇0⎇⎇| | | |␈←
1094⎇⎇␈→60⎇0⎇⎇| | 1 1 1 | |␈←
1095⎇⎇␈→60⎇0⎇⎇| | |␈π_␈π_␈π_␈π_␈π_␈π_␈π_ |␈←
1096⎇⎇␈→60⎇0⎇⎇| | 1/T2 2/T2 3/T2 | | |␈←
1097⎇⎇␈→60⎇0⎇⎇| | 2 2| | |␈←
1098⎇⎇␈→60⎇0⎇⎇| | 2/T2 6/T2 | | |␈←
1099⎇⎇␈→60⎇0⎇⎇| |␈π_␈π_␈π_␈π_␈π_␈π_␈π_␈π_␈π_␈π_␈π_␈π_␈π_␈π_␈π_␈π_␈π_␈π_| " " | |␈←
1100⎇⎇␈→60⎇0⎇⎇| | | |␈←
1101⎇⎇␈→60⎇0⎇⎇| | | |␈←
1102⎇⎇␈→60⎇0⎇⎇| | | -3/Tn 4/Tn |␈←
1103⎇⎇␈→60⎇0⎇⎇| | | 2 2|␈←
1104⎇⎇␈→60⎇0⎇⎇| |␈π_␈π_␈π_␈π_␈π_␈π_␈π_| 6/Tn -12/Tn |␈←
1105⎇⎇␈→60⎇0⎇⎇| |␈←
1106⎇⎇␈→60⎇0⎇⎇| 1 -1 |␈←
1107⎇⎇␈→60⎇0⎇⎇␈←␈→60⎇-1000⎇⎇[Eq. 5.23]␈←
1108⎇⎇ | ~␈-1⎇VO␈-1⎇-1 | | A13 |
1109⎇⎇ |␈π_␈π_0␈π_␈π_| |␈π_A14␈π_|
1110⎇⎇ | 0 | | A21 |
1111⎇⎇ | ~␈-1⎇VO␈-1⎇-2 | | A22 |
1112⎇⎇July 20, 1972␈→60⎇-1000⎇⎇5.2 POLYNOMIALS␈←
1113⎇⎇␈→60⎇0⎇⎇ 16482⎇␈←␈→60⎇-1000⎇⎇␈←
1114⎇⎇ |␈π_␈π_0␈π_␈π_| = | COEFF | * |␈π_A23␈π_|␈→60⎇-1000⎇⎇[Eq. 5.24]␈←
1115⎇⎇ | | | |
1116⎇⎇ | " " | | " " |
1117⎇⎇ |␈π_␈π_␈π_␈π_␈π_| |␈π_␈π_␈π_␈π_␈π_|
1118⎇⎇ | 0 | | An3 |
1119⎇⎇ | ~␈-1⎇VO␈-1⎇-4 | | An4 |
1120⎇⎇where the blocks indicated in Equation 5.23 and Equation
1121⎇⎇5.24 may be repeated for each additional point that the
1122⎇⎇trajectory must pass through.
1123⎇⎇In the last trajectory segment we solved in terms of t'. As
1124⎇⎇we wish to evaluate all the trajectories similarly we
1125⎇⎇substitute:
1126⎇⎇␈→60⎇0⎇⎇t = t' + 1␈←␈→60⎇-1000⎇⎇[Eq. 5.25]␈←
1127⎇⎇we have:
1128⎇⎇␈→60⎇0⎇⎇O␈-1⎇- = A4n*t'␈↑4␈↓ + A3n*t'␈↑3␈↓ + A2n*t'␈↑2␈↓ + A1n*t' + A0n␈←
1129⎇⎇␈→60⎇0⎇⎇␈←␈→60⎇-1000⎇⎇[Eq. 5.26]␈←
1130⎇⎇substituting for t' in Equation 5.26 from Equation 5.25 we
1131⎇⎇obtain:
1132⎇⎇ O␈-1⎇- = A4n*t␈↑4␈↓
1133⎇⎇ +(-4*A4n + A3n)*t␈↑3␈↓
1134⎇⎇ +(6*A4n -3*A3n +A2n)*t␈↑2␈↓
1135⎇⎇ +(-4*A4n +3*A3n -2*A2n +A1n)*t
1136⎇⎇ +(A4n -A3n +A2n -A1n +A0n)
1137⎇⎇␈→60⎇-1000⎇⎇[Eq. 5.27]␈←
1138⎇⎇July 20, 1972␈→60⎇-1000⎇⎇5.2 POLYNOMIALS␈←
1139⎇⎇␈→60⎇0⎇⎇ 16483⎇␈←␈→60⎇-1000⎇⎇␈←
1140⎇⎇from which we can obtain the coefficients of t.
1141⎇⎇5.3 TRAJECTORY EXTREMA
1142⎇⎇Intermediate trajectory segments are described by third
1143⎇⎇order polynomials, one such polynomial for each joint:
1144⎇⎇␈→60⎇0⎇⎇O␈-1⎇- = Ai3*t␈↑3␈↓ + Ai2*t␈↑2␈↓ + Ai1*t +Ai0␈←␈→60⎇-1000⎇⎇[Eq. 5.28]␈←
1145⎇⎇the derivative is:
1146⎇⎇␈→60⎇0⎇⎇O␈-1⎇-␈-1⎇␈↑.␈↓ = 3*Ai3*t␈↑2␈↓ + 2*Ai2*t + Ai1␈←␈→60⎇-1000⎇⎇[Eq. 5.29]␈←
1147⎇⎇The times of the extrema are given as the roots of Equation
1148⎇⎇5.29:
1149⎇⎇␈→60⎇0⎇⎇t = -1/3 Ai2/Ai3 ␈π+ {(1/3 Ai2/A13)␈↑2␈↓ - 1/3 Ai1/Ai3}␈↑1/2␈↓␈←
1150⎇⎇␈→60⎇0⎇⎇␈←␈→60⎇-1000⎇⎇[Eq. 5.30]␈←
1151⎇⎇If the discriminant is positive and 0 < t < 1 then an
1152⎇⎇extremum exists and it can be evaluated by Equation 5.28.
1153⎇⎇In the case of the initial and final trajectory segments the
1154⎇⎇trajectories are described by fourth order polynomials
1155⎇⎇(Equation 5.1) with the low order terms missing (see
1156⎇⎇Equation 5.4, 5.5, 5.11, and 5.12).
1157⎇⎇␈→60⎇0⎇⎇O␈-1⎇- = Ai4*t␈↑4␈↓ + Ai3*t␈↑3␈↓ + Ai0␈←␈→60⎇-1000⎇⎇[Eq. 5.31]␈←
1158⎇⎇and the derivative by:
1159⎇⎇␈→60⎇0⎇⎇O␈-1⎇-␈-1⎇␈↑.␈↓ = 4*Ai4*t␈↑3␈↓ + 3*Ai3*t␈↑2␈↓␈←␈→60⎇-1000⎇⎇[Eq. 5.32]␈←
1160⎇⎇The time of the extremum is given as the roots of Equation
1161⎇⎇5.32:
1162⎇⎇July 20, 1972␈→60⎇-1000⎇⎇5.3 EXTREMA␈←
1163⎇⎇␈→60⎇0⎇⎇ 16485⎇␈←␈→60⎇-1000⎇⎇␈←
1164⎇⎇␈→60⎇0⎇⎇t = -3/4 Ai3/Ai4␈←␈→60⎇-1000⎇⎇[Eq. 5.33]␈←
1165⎇⎇if 0 < t < 1 then an extremum exists and the value of O␈-1⎇- is:
1166⎇⎇␈→60⎇0⎇⎇O␈-1⎇- = -1/4 Ai3*t␈↑3␈↓␈←␈→60⎇-1000⎇⎇[Eq. 5.34]␈←
1167⎇⎇If the value of O␈-1⎇- at the extremum causes an appreciable
1168⎇⎇overshoot. then the trajectory is required to pass through
1169⎇⎇the liftoff/set-down point once more Figure 5.5).
1170⎇⎇5.4 CONTINUOUS MOTION
1171⎇⎇In Subsection 5.1 we mentioned trajectories along space
1172⎇⎇curves, where arm solutions are obtained at regular
1173⎇⎇intervals along the curve and then a trajectory is planned
1174⎇⎇through these positions. Such trajectories are not checked
1175⎇⎇for overshoot of physical limits as they are more
1176⎇⎇continuous. There is a special case of these trajectories
1177⎇⎇in which the first and last arm positions are the same. (see
1178⎇⎇Figure 5.6). Here we have a circular arm motion such as
1179⎇⎇turning a crank, the position of the hand is described by an
1180⎇⎇angle; the orientation remains the same during the motion.
1181⎇⎇Solutions are obtained every 60 degrees. In the case of the
1182⎇⎇beginning and end, two additional positions are specified,
1183⎇⎇one at 20 and one at 340 degrees respectively. The arm is
1184⎇⎇given the same period of time for each of the 8 segments,
1185⎇⎇and thus the arm will accelerate from rest, move around the
1186⎇⎇circle and come back to rest. The first and last small
1187⎇⎇segments ensure the smooth departure and arrival.
1188⎇⎇For continuous motion we provide an alternate path from
1189⎇⎇position 6 through position 0 to position 2 (see Figure 5.6)
1190⎇⎇requiring continuity of both velocity and acceleration at
1191⎇⎇these points. The arm is caused to rotate continuously
1192⎇⎇having first been started from point 0 by causing it to move
1193⎇⎇along the alternate trajectory at point 6. When it is
1194⎇⎇desired to stop it is not be diverted at the "switch" at
1195⎇⎇position 6 but allowed to stop by moving through position 7
1196⎇⎇to 8.
1197⎇⎇In the following matrix equation the x's represent non zero
1198⎇⎇elements of Equation 5.23 Equation 131⎇ is for the simple
1199⎇⎇July 20, 1972␈→60⎇-1000⎇⎇5.4 LOOPING␈←
1200⎇⎇␈→60⎇0⎇⎇ 16488⎇␈←␈→60⎇-1000⎇⎇␈←
1201⎇⎇case and Equation 369⎇ is in the case of looping providing
1202⎇⎇for the solution of two additional fourth order trajectory
1203⎇⎇segments 9 and 10.
1204⎇⎇␈→60⎇0⎇⎇| ~␈-1⎇VO␈-1⎇-1 | | XX | | A13 |␈←
1205⎇⎇␈→60⎇0⎇⎇| 0 | | XXX | | A14 |␈←
1206⎇⎇␈→60⎇0⎇⎇| 0 | | XX X | | A21 |␈←
1207⎇⎇␈→60⎇0⎇⎇| ~␈-1⎇VO␈-1⎇-2 | | XXX | | A22 |␈←
1208⎇⎇␈→60⎇0⎇⎇| 0 | | XXXX | | A23 |␈←
1209⎇⎇␈→60⎇0⎇⎇| 0 | | XX X | | A31 |␈←
1210⎇⎇␈→60⎇0⎇⎇| ~␈-1⎇VO␈-1⎇-3 | | XXX | | A32 |␈←
1211⎇⎇␈→60⎇0⎇⎇| 0 | | XXXX | | A33 |␈←
1212⎇⎇␈→60⎇0⎇⎇| 0 | | XX X | | A41 |␈←
1213⎇⎇␈→60⎇0⎇⎇| ~␈-1⎇VO␈-1⎇-4 | = | XXX | * | A42 |␈←
1214⎇⎇␈→60⎇0⎇⎇| 0 | | XXXX | | A43 |␈←
1215⎇⎇␈→60⎇0⎇⎇| 0 | | XX X | | A51 |␈←
1216⎇⎇␈→60⎇0⎇⎇| ~␈-1⎇VO␈-1⎇-5 | | XXX | | A52 |␈←
1217⎇⎇␈→60⎇0⎇⎇| 0 | | XXXX | | A53 |␈←
1218⎇⎇␈→60⎇0⎇⎇| 0 | | XX X | | A61 |␈←
1219⎇⎇␈→60⎇0⎇⎇| ~␈-1⎇VO␈-1⎇-6 | | XXX | | A62 |␈←
1220⎇⎇␈→60⎇0⎇⎇| 0 | | XXXX | | A63 |␈←
1221⎇⎇␈→60⎇0⎇⎇| 0 | | XX X | | A71 |␈←
1222⎇⎇␈→60⎇0⎇⎇| ~␈-1⎇VO␈-1⎇-7 | | XXX | | A72 |␈←
1223⎇⎇␈→60⎇0⎇⎇| 0 | | XXXXX | | A73 |␈←
1224⎇⎇␈→60⎇0⎇⎇| 0 | | XXXX | | A83 |␈←
1225⎇⎇␈→60⎇0⎇⎇| ~␈-1⎇VO␈-1⎇-8 | | XX | | A84 |␈←
1226⎇⎇␈→60⎇0⎇⎇␈←␈→60⎇-1000⎇⎇[Eq. 5.35]␈←
1227⎇⎇and in the case of looping, the blocks of elements marked as
1228⎇⎇Y's and Z's are the same.
1229⎇⎇␈→60⎇0⎇⎇| ~␈-1⎇VO␈-1⎇-1 | | XX | | A13 |␈←
1230⎇⎇␈→60⎇0⎇⎇| 0 | | XXX | | A14 |␈←
1231⎇⎇␈→60⎇0⎇⎇| 0 | | XX X | | A21 |␈←
1232⎇⎇␈→60⎇0⎇⎇| ~␈-1⎇VO␈-1⎇-2 | | XXX | | A22 |␈←
1233⎇⎇␈→60⎇0⎇⎇| 0 | | XXXY | | A23 |␈←
1234⎇⎇␈→60⎇0⎇⎇| 0 | | XX Y | | A31 |␈←
1235⎇⎇␈→60⎇0⎇⎇| ~␈-1⎇VO␈-1⎇-3 | | XXX | | A32 |␈←
1236⎇⎇␈→60⎇0⎇⎇| 0 | | XXXX | | A33 |␈←
1237⎇⎇␈→60⎇0⎇⎇| 0 | | XX X | | A41 |␈←
1238⎇⎇␈→60⎇0⎇⎇| ~␈-1⎇VO␈-1⎇-4 | | XXX | | A42 |␈←
1239⎇⎇␈→60⎇0⎇⎇| 0 | | XXXX | | A43 |␈←
1240⎇⎇␈→60⎇0⎇⎇| 0 | | XX X | | A51 |␈←
1241⎇⎇␈→60⎇0⎇⎇| ~␈-1⎇VO␈-1⎇-5 | = | XXX | * | A52 |␈←
1242⎇⎇␈→60⎇0⎇⎇| 0 | | XXXX | | A53 |␈←
1243⎇⎇␈→60⎇0⎇⎇| 0 | | XX X | | A61 |␈←
1244⎇⎇July 20, 1972␈→60⎇-1000⎇⎇5.4 LOOPING␈←
1245⎇⎇␈→60⎇0⎇⎇ 16489⎇␈←␈→60⎇-1000⎇⎇␈←
1246⎇⎇␈→60⎇0⎇⎇| ~␈-1⎇VO␈-1⎇-6 | | XXX | | A62 |␈←
1247⎇⎇␈→60⎇0⎇⎇| 0 | | ZZZX | | A63 |␈←
1248⎇⎇␈→60⎇0⎇⎇| 0 | | ZZ X | | A71 |␈←
1249⎇⎇␈→60⎇0⎇⎇| ~␈-1⎇VO␈-1⎇-7 | | XXX | | A72 |␈←
1250⎇⎇␈→60⎇0⎇⎇| 0 | | XXXXX | | A73 |␈←
1251⎇⎇␈→60⎇0⎇⎇| 0 | | XXXX | | A83 |␈←
1252⎇⎇␈→60⎇0⎇⎇| ~␈-1⎇VO␈-1⎇-8 | | XX | | A84 |␈←
1253⎇⎇␈→60⎇0⎇⎇| 0 | | ZZZ X | | A91 |␈←
1254⎇⎇␈→60⎇0⎇⎇| 0 | | ZZ X | | A92 |␈←
1255⎇⎇␈→60⎇0⎇⎇| ~␈-1⎇VO␈-1⎇-9 | | XXXX | | A93 |␈←
1256⎇⎇␈→60⎇0⎇⎇| 0 | | XXXXX | | A94 |␈←
1257⎇⎇␈→60⎇0⎇⎇| 0 | | XXX X | | A101|␈←
1258⎇⎇␈→60⎇0⎇⎇| ~␈-1⎇VO␈-1⎇-10| | XXXX | | A102|␈←
1259⎇⎇␈→60⎇0⎇⎇| 0 | | Y XXXX | | A103|␈←
1260⎇⎇␈→60⎇0⎇⎇| 0 | | Y XXX | | A104|␈←
1261⎇⎇␈→60⎇0⎇⎇␈←␈→60⎇-1000⎇⎇[Eq. 5.36]␈←
1262⎇⎇Equation 5.36 is the same as Equation 5.35 except for the
1263⎇⎇addition of the last eight equations for segments 9 and 10.
1264⎇⎇These are fourth order polynomials due to the extra end
1265⎇⎇continuity constraints. The groups of non zero elements in
1266⎇⎇the lower triangle of Equation 5.36 relate to the continuity
1267⎇⎇requirements at points 2 and 6 (see Figure 5.6).
1268⎇⎇5.5 DIFFERENTIAL MOTION
1269⎇⎇The arm can also make differential motions where all six
1270⎇⎇joints are required to change a certain amount in a given
1271⎇⎇July 20, 1972␈→60⎇-1000⎇⎇5.5 DIFFERENTIAL␈←
1272⎇⎇␈→60⎇0⎇⎇ 16491⎇␈←␈→60⎇-1000⎇⎇␈←
1273⎇⎇time. In this case all the joints are driven together to
1274⎇⎇make the change in the specified time. The changes of joint
1275⎇⎇angles are determined as in Subsection 3.3.
1276⎇⎇Joint angle O␈-1⎇- as a function g(t) of normalized time t, for a
1277⎇⎇change of ~␈-1⎇VO␈-1⎇- is:
1278⎇⎇␈→60⎇0⎇⎇t = t/Tm␈←␈→60⎇-1000⎇⎇[Eq. 5.37]␈←
1279⎇⎇␈→60⎇0⎇⎇O␈-1⎇-=g(t)=~␈-1⎇VO␈-1⎇-*t␈↑3␈↓( 6t␈↑2␈↓ - 15t + 10)␈←␈→60⎇-1000⎇⎇[Eq. 5.38]␈←
1280⎇⎇This gives zero initial and final acceleration and velocity.
1281⎇⎇This type of motion has all the undesirable properties
1282⎇⎇described in Subsection 5.1 and is only suitable when small
1283⎇⎇changes are to be made, as in correcting the hand position
1284⎇⎇during visual servoing. Such motions are of course much
1285⎇⎇simpler to plan than regular trajectory controlled motion.
1286⎇⎇July 20, 1972␈→60⎇-1000⎇⎇␈←
1287⎇⎇␈→60⎇0⎇⎇ 16492⎇␈←␈→60⎇-1000⎇⎇␈←
1288⎇⎇␈→60⎇0⎇⎇6.0 SERVO␈←
1289⎇⎇In this section we relate position error to joint torque in
1290⎇⎇the arm servo. We then discuss the model of the joint drive
1291⎇⎇in order to convert joint torque into motor drive. In the
1292⎇⎇final section provision for degrees of freedom of the arm as
1293⎇⎇it is servoed is considered.
1294⎇⎇6.1 FEEDBACK LOOP
1295⎇⎇In this section we will describe the servo response. We
1296⎇⎇will treat the system as continuous, ignoring the effects of
1297⎇⎇sampling, based on the assumption that the sampling period
1298⎇⎇is much less than the response time of the arm. At the end
1299⎇⎇of the section we will ckeck that this assumption is valid.
1300⎇⎇Time is normalized to the sampling period which has the
1301⎇⎇effect of scaling the link inertia up by f↑2 where f is the
1302⎇⎇sampling frequency. The Laplace transform is used
1303⎇⎇throughout represented by F(s).
1304⎇⎇The set point for each joint of the arm is obtained by
1305⎇⎇evaluating the appropriate part trajectory polynomial for
1306⎇⎇the required time. The velocity and acceleration could also
1307⎇⎇be evaluated as the first and second derivatives of the
1308⎇⎇polynomial but instead they are approximated by the first
1309⎇⎇and second differences as this saves time.
1310⎇⎇The position error is the observed position O␈-1⎇- less the
1311⎇⎇required value O␈-1⎇-s. Likewise the velocity error is the
1312⎇⎇observed velocity less the required velocity. Position
1313⎇⎇feedback is applied to decrease position error and velocity
1314⎇⎇feedback is used to provide damping.
1315⎇⎇The feedback loop is shown in Figure 6.1 The arm is
1316⎇⎇represented by 1/s␈↑2␈↓J, where J is the effective link inertia
1317⎇⎇a function of arm configuration, and T(s) is an external
1318⎇⎇disturbing torque. The set point R(s) is subtracted from
1319⎇⎇the current position to obtain the position error E(s) and
1320⎇⎇is multiplied by s to obtain the error velocity. There are
1321⎇⎇two feedback gains ke and kv, position and velocity
1322⎇⎇respectively.
1323⎇⎇By writing the loop equation we can obtain the system
1324⎇⎇response:
1325⎇⎇July 20, 1972␈→60⎇-1000⎇⎇6.1 FEEDBACK␈←
1326⎇⎇␈→60⎇0⎇⎇ 16496⎇␈←␈→60⎇-1000⎇⎇␈←
1327⎇⎇␈→60⎇0⎇⎇E(s)= (-s␈↑2␈↓J)/(s␈↑2␈↓J + skv + ke)*R(s) + 1/(s␈↑2␈↓ + skv + ke)*T(s)␈←
1328⎇⎇␈→60⎇0⎇⎇␈←␈→60⎇-1000⎇⎇[Eq. 6.1]␈←
1329⎇⎇and the condition for critical damping is:
1330⎇⎇␈→60⎇0⎇⎇kv = 2(J*ke)␈↑1/2␈↓␈←␈→60⎇-1000⎇⎇[Eq. 6.2]␈←
1331⎇⎇It can be seen that the system response is dependent on J as
1332⎇⎇would be expected. Because the effective link inertia J can
1333⎇⎇vary by 10:1 as the arm configuration changes, we are unable
1334⎇⎇to maintain a given response (see Equation 6.2) independent
1335⎇⎇of arm configuration. If however we add a gain of -J as
1336⎇⎇shown in Figure 6.2 then we obtain:
1337⎇⎇␈→60⎇0⎇⎇E(s)= (-s␈↑2␈↓)/(s␈↑2␈↓ + skv + ke)*R(s) + 1/(s␈↑2␈↓ + skv + ke)*T(s)/J␈←
1338⎇⎇␈→60⎇0⎇⎇␈←␈→60⎇-1000⎇⎇[Eq. 6.3]␈←
1339⎇⎇and the condition for critical damping is:
1340⎇⎇␈→60⎇0⎇⎇kv = 2*(ke)␈↑1/2␈↓␈←␈→60⎇-1000⎇⎇[Eq. 6.4]␈←
1341⎇⎇It can be seen that the servo response is now independent of
1342⎇⎇arm configuration.
1343⎇⎇The principal disturbing torque is that due to gravity,
1344⎇⎇causing a large position error, especially in the case of
1345⎇⎇joint 2. If we were able to add a term equal to the negative
1346⎇⎇of the gravity loading Tg (see Figure 6.3) then we would
1347⎇⎇obtain the same system response as in Equation 6.3 except
1348⎇⎇that T is now Te, the external disturbing torque, less the
1349⎇⎇gravity dependent torque, and reducing the position error.
1350⎇⎇␈→60⎇0⎇⎇E(s)= (-s␈↑2␈↓)/(s␈↑2␈↓ + skv + ke)*R(s) + 1/(s␈↑2␈↓ + skv + ke)*Te(s)/J␈←
1351⎇⎇␈→60⎇0⎇⎇␈←␈→60⎇-1000⎇⎇[Eq. 6.5]␈←
1352⎇⎇We can compensate for the effect of acceleration of the set
1353⎇⎇point R(s), the first term in Equation 6.5, if we add a term
1354⎇⎇July 20, 1972␈→60⎇-1000⎇⎇6.1 FEEDBACK␈←
1355⎇⎇␈→60⎇0⎇⎇ 16500⎇␈←␈→60⎇-1000⎇⎇␈←
1356⎇⎇s␈↑2␈↓R(s) (seeFigure 6.4) and obtain a system response:
1357⎇⎇␈→60⎇0⎇⎇E(s)= 1/(s␈↑2␈↓ + skv + ke)*T(s)/J␈←␈→60⎇-1000⎇⎇[Eq. 6.6]␈←
1358⎇⎇The gain of -J and the torque Tg are obtained from Equation
1359⎇⎇3.33 which we restate here:
1360⎇⎇ Fi = ~␈-1⎇>␈-1⎇␈π_␈-1⎇␈↑n␈↓␈-2⎇␈↓j=i␈↑[~␈-1⎇>␈-1⎇␈π_␈-1⎇␈↑j␈↓␈-2⎇␈↓k=1␈↑Trace(| Ujk|*| Hj|*| Uji|␈↑T␈↓*qk␈-2⎇␈↑..␈↓)
1361⎇⎇ +~␈-1⎇>␈-1⎇␈π_␈-1⎇␈↑j␈↓␈-2⎇␈↓k=1␈↑ ~␈-1⎇>␈-1⎇␈π_␈-1⎇␈↑j␈↓␈-2⎇␈↓l=1␈↑Trace(| Ujkl|*| Hj|*| Uji|␈↑T␈↓*q␈-1⎇␈↑.␈↓j*q␈-1⎇␈↑.␈↓k)
1362⎇⎇ -mj*| G |*| Uji|*| R␈-1⎇␈↑_␈↓j]␈→60⎇-1000⎇⎇[Eq 3.33]␈←
1363⎇⎇What is desired is to obtain an expression of the form:
1364⎇⎇␈→60⎇0⎇⎇ Fi = Ji * qi␈-2⎇␈↑..␈↓ + Tg␈←␈→60⎇-1000⎇⎇[Eq. 6.7]␈←
1365⎇⎇given an arm configuration qi.
1366⎇⎇The velocity dependent terms of Equation 3.33 are only
1367⎇⎇significant at high speed and small for the arm we are
1368⎇⎇using. We will ignore the second term of Equation 3.33,
1369⎇⎇although it could be included with the third term as the
1370⎇⎇values of q␈-1⎇␈↑.␈↓j and q␈-1⎇␈↑.␈↓k are known from the trajectory.
1371⎇⎇We may interchange the order of summation of Equation 3.33
1372⎇⎇to obtain:
1373⎇⎇ Fi = ~␈-1⎇>␈-1⎇␈π_␈-1⎇␈↑6␈↓␈-2⎇␈↓k=1␈↑ ~␈-1⎇>␈-1⎇␈π_␈-1⎇␈↑6␈↓␈-2⎇␈↓j=i␈↑Trace(| Ujk|*|Hj|*|Uji|␈↑T␈↓)*qk␈-2⎇␈↑..␈↓
1374⎇⎇ -~␈-1⎇>␈-1⎇␈π_␈-1⎇␈↑6␈↓␈-2⎇␈↓j=i␈↑mj*| G |*| Uji|*| R␈-1⎇␈↑_␈↓j]␈→60⎇-1000⎇⎇[Eq. 6.8]␈←
1375⎇⎇July 20, 1972␈→60⎇-1000⎇⎇6.1 FEEDBACK␈←
1376⎇⎇␈→60⎇0⎇⎇ 16501⎇␈←␈→60⎇-1000⎇⎇␈←
1377⎇⎇or
1378⎇⎇␈→60⎇0⎇⎇Fi = ~␈-1⎇>␈-1⎇␈π_␈-1⎇␈↑6␈↓␈-2⎇␈↓k=1␈↑ Cik qk␈-2⎇␈↑..␈↓ + Ci␈←␈→60⎇-1000⎇⎇[Eq. 6.9]␈←
1379⎇⎇where
1380⎇⎇␈→60⎇0⎇⎇ Cik= ~␈-1⎇>␈-1⎇␈π_␈-1⎇␈↑6␈↓␈-2⎇␈↓j=i␈↑Trace(| Ujk|*| Hj|*| Uji|␈↑T␈↓)␈←␈→60⎇-1000⎇⎇[Eq. 6.10]␈←
1381⎇⎇and
1382⎇⎇␈→60⎇0⎇⎇ Ci = -~␈-1⎇>␈-1⎇␈π_␈-1⎇␈↑6␈↓␈-2⎇␈↓j=i␈↑mj*| G |*| Uji|*| R␈-1⎇␈↑_␈↓j]␈←␈→60⎇-1000⎇⎇[Eq. 6.11]␈←
1383⎇⎇One further simplification is that we may disregard the
1384⎇⎇terms of Equation 6.9 in which i≠k as they are very small,
1385⎇⎇to obtain:
1386⎇⎇␈→60⎇0⎇⎇ Fi = Ji * qi␈-2⎇␈↑..␈↓ + Tg␈←␈→60⎇-1000⎇⎇[Eq. 6.12]␈←
1387⎇⎇where
1388⎇⎇␈→60⎇0⎇⎇ Ji = ~␈-1⎇>␈-1⎇␈π_␈-1⎇␈↑6␈↓␈-2⎇␈↓j=i␈↑Trace(| Uji|*| Hj|*| Uji|␈↑T␈↓)␈←␈→60⎇-1000⎇⎇[Eq. 6.13]␈←
1389⎇⎇and Tg as Ci in Equation 6.11
1390⎇⎇Ji is the effective joint inertia and Tg is the constant
1391⎇⎇term due to gravity.
1392⎇⎇The servo is compensated for system response, under varying
1393⎇⎇arm configuration, for gravity loading and for the
1394⎇⎇acceleration of the set point r.
1395⎇⎇Three plots of position error v. time for joint 2 are shown:
1396⎇⎇Figure 6.5, Figure 6.6, and Figure 6.7.
1397⎇⎇July 20, 1972␈→60⎇-1000⎇⎇6.1 FEEDBACK␈←
1398⎇⎇␈→60⎇0⎇⎇ 16505⎇␈←␈→60⎇-1000⎇⎇␈←
1399⎇⎇In Figure 6.5 there is no gravity or feedback compensation.
1400⎇⎇In Figure 6.6 the gravity term is added, and finally in
1401⎇⎇Figure 6.7 the acceleration compensation is included.
1402⎇⎇The steady state error for a given error torque Te is:
1403⎇⎇␈→60⎇0⎇⎇Ess = Te/( J * ke )␈←␈→60⎇-1000⎇⎇[Eq. 6.14]␈←
1404⎇⎇We would like the arm to be fairly stiff when it is servoing
1405⎇⎇and a value of 50 oz/in is the minimum acceptable. if a
1406⎇⎇lesser value is used the arm moves very sloppily. For this
1407⎇⎇value of stiffness the values of ke can be calculated by
1408⎇⎇means of Equation 6.14. Representative values of kv based on
1409⎇⎇critical damping may then be calculated (Equation 6.4) and
1410⎇⎇this will give us the lower limits on ke. and kv.
1411⎇⎇There is, however, some noise in position measurement and if
1412⎇⎇we are to obtain velocity by differencing observed position
1413⎇⎇readings then the same noise is in the velocity
1414⎇⎇determination. The primary source of noise is the
1415⎇⎇quantizing noise of the analog to digital converter used to
1416⎇⎇determine position. The A/D converter is a 12 bit device and
1417⎇⎇thus has a relative error of 1/4096. The equivalent noise
1418⎇⎇torque at the joint is given by:
1419⎇⎇␈→60⎇0⎇⎇Tn = k * J * e␈←␈→60⎇-1000⎇⎇[Eq. 6.15]␈←
1420⎇⎇where k is either ke if the velocity is determined
1421⎇⎇separately or ke+kv if the velocity is determined by
1422⎇⎇differencing, and e is the quantizing noise. A noise torque
1423⎇⎇of approximately F0/2 (see Subsection 202⎇) seems to be
1424⎇⎇acceptable and this gives an upper bound on k.
1425⎇⎇Based on this information we can evaluate Equations 6.14,
1426⎇⎇6.4 and 6.15 for each joint:
1427⎇⎇␈→60⎇0⎇⎇JOINT ke␈-2⎇␈π_␈π_ kv␈-2⎇␈π_␈π_ k␈-1⎇␈↑_␈↓␈←
1428⎇⎇␈→60⎇0⎇⎇ 1 0.038 0.39 0.18␈←
1429⎇⎇␈→60⎇0⎇⎇ 2 0.032 0.36 0.33␈←
1430⎇⎇␈→60⎇0⎇⎇ 3 0.005 0.14 0.75␈←
1431⎇⎇␈→60⎇0⎇⎇ 4 0.06 0.49 0.49␈←
1432⎇⎇␈→60⎇0⎇⎇ 5 0.06 0.49 1.6␈←
1433⎇⎇␈→60⎇0⎇⎇ 6 0.06 0.49 50.0␈←
1434⎇⎇July 20, 1972␈→60⎇-1000⎇⎇6.1 FEEDBACK␈←
1435⎇⎇␈→60⎇0⎇⎇ 16506⎇␈←␈→60⎇-1000⎇⎇␈←
1436⎇⎇The second column is based on Equation 6.14 and the third on
1437⎇⎇Equation 6.4 These are the minimum acceptable gains. The
1438⎇⎇fourth column is from Equation 6.15 and represents the
1439⎇⎇maximum allowable gain if the information is to be obtained
1440⎇⎇by means of position measurement using the available A/D
1441⎇⎇converter. In the case of Joints 1 and 2 it can be seen
1442⎇⎇that an alternative means must be used to obtain velocity
1443⎇⎇information although the position information is acceptable
1444⎇⎇using the A/D converter. For these two joints a tachometer
1445⎇⎇generator is used to measure the velocity, which has much
1446⎇⎇lower noise than velocity obtained by differencing position
1447⎇⎇measurements.
1448⎇⎇We made an assumption at the beginning of this section that
1449⎇⎇the sampling frequency was much higher than the frequency
1450⎇⎇response of the arm. We can obtain the frequency response
1451⎇⎇from Equation 6.6 by finding the inverse transform. This is
1452⎇⎇found to be:
1453⎇⎇␈→60⎇0⎇⎇frequency = kv * f / 2␈←␈→60⎇-1000⎇⎇[Eq. 6.16]␈←
1454⎇⎇As maximum kv that we require is 0.49 this give us a
1455⎇⎇response of 1/4 the sampling frequency.
1456⎇⎇At the beginning and end of each trajectory and at the end
1457⎇⎇of each part trajectory, the values of Ji and Tg are
1458⎇⎇evaluated (Equation 6.12). They are then given to the servo
1459⎇⎇along with the part trajectory polynomial coefficients. As
1460⎇⎇the trajectory is executed the values of Ji and Tg are
1461⎇⎇linearly interpolated.
1462⎇⎇Although this gives an acceptable response from the point of
1463⎇⎇view of stiffness, the gain is too low to maintain the high
1464⎇⎇positional tolerance of ␈π-0.05 in, which we are just able to
1465⎇⎇measure using the 12 bit A/D converter. In order to achieve
1466⎇⎇this error tolerance the position error is integrated when
1467⎇⎇the arm has reached the end of its trajectory. When the
1468⎇⎇position error of a joint is within tolerance the brake for
1469⎇⎇that joint is applied and the joint is no longer servoed.
1470⎇⎇When all the joints are within the error tolerance the
1471⎇⎇trajectory has been executed.
1472⎇⎇If the arm is to move a heavy load, its predicted effects
1473⎇⎇may be taken into account by increasing the effective mass
1474⎇⎇and inertia of the last link of the arm before evaluating
1475⎇⎇Equation 6.12. Similarly if the hand is to exert a given
1476⎇⎇July 20, 1972␈→60⎇-1000⎇⎇6.1 FEEDBACK␈←
1477⎇⎇␈→60⎇0⎇⎇ 16507⎇␈←␈→60⎇-1000⎇⎇␈←
1478⎇⎇force or moment then the equivalent arm torque (see
1479⎇⎇Subsection 3.5) is added to Tg.
1480⎇⎇6.2 MOTOR DRIVE
1481⎇⎇The output of the servo equation is a torque to be applied
1482⎇⎇at the joint. Each joint has an electric motor drive and a
1483⎇⎇harmonic drive gear reduction. The motors are driven by a
1484⎇⎇pulse-width modulated voltage signal. The output of the
1485⎇⎇computer is this pulse␈π_width and the polarity. The drive
1486⎇⎇module relates torque to drive voltage pulse-width.
1487⎇⎇The motors are driven by a 360 Hertz pulse-width modulated
1488⎇⎇voltage source. The program output "h" is the relative "on"
1489⎇⎇time of this signal. If we plot an experimental curve of "h"
1490⎇⎇v. joint torque we obtain two discontinuous curves depending
1491⎇⎇on the joint velocity. see Figure 6.8
1492⎇⎇This curve can be explained in terms of friction effects;
1493⎇⎇load dependent, causing the two curves to diverge, and load
1494⎇⎇independent, causing the separation at the two curves at the
1495⎇⎇origin. The electrical motor time constant also affects the
1496⎇⎇shape of the curve near the origin. Curves determined
1497⎇⎇experimentally are supplied to the servo program in the
1498⎇⎇following piecewise linear form, (see Figure 6.9)
1499⎇⎇where:
1500⎇⎇ V0 is the motor drive at which the joint will move
1501⎇⎇ at constant velocity exerting zero force in the
1502⎇⎇ direction of motion;
1503⎇⎇ F0 is the force that the joint will exert at drive
1504⎇⎇ level V0 but with a negative velocity;
1505⎇⎇ the slopes and slope differences are obtained from
1506⎇⎇ the experimental curves.
1507⎇⎇When the velocity is very low the direction of intended
1508⎇⎇motion is substituted for the velocity.
1509⎇⎇One other factor taken into account is the back emf of the
1510⎇⎇motor. The value of "h" is the ratio of required voltage to
1511⎇⎇supply voltage. The supply voltage is simply augmented by
1512⎇⎇the computed back emf before "h" is calculated.
1513⎇⎇July 20, 1972␈→60⎇-1000⎇⎇6.2 DRIVE␈←
1514⎇⎇␈→60⎇0⎇⎇ 16511⎇␈←␈→60⎇-1000⎇⎇␈←
1515⎇⎇When the velocity is non zero the output torque is
1516⎇⎇predictable but at zero velocity and with zero intended
1517⎇⎇motion the error in the output torque is half the horizontal
1518⎇⎇displacement between the two curves at the origin. this is
1519⎇⎇F0/2. The values for this error torque at a typical arm
1520⎇⎇configuration in terms of force at the hand are:
1521⎇⎇␈→60⎇0⎇⎇JOINT F0 oz.FORCE oz. ␈←
1522⎇⎇␈→60⎇0⎇⎇ 1 400 ␈π-10 ␈←
1523⎇⎇␈→60⎇0⎇⎇ 2 700 ␈π-20 ␈←
1524⎇⎇␈→60⎇0⎇⎇ 3 70 ␈π-35 ␈←
1525⎇⎇␈→60⎇0⎇⎇ 4 100 ␈π-5 ␈←
1526⎇⎇␈→60⎇0⎇⎇ 5 160 ␈π-8 ␈←
1527⎇⎇␈→60⎇0⎇⎇ 6 100 ␈←
1528⎇⎇It can be seen that the arm can exert forces with a typical
1529⎇⎇tolerance of ␈π-10 oz.
1530⎇⎇6.3 PARTIALLY CONSTRAINED MOTION
1531⎇⎇If we require the arm to exhibit a degree of freedom along a
1532⎇⎇given direction or about a given axis at the hand then we
1533⎇⎇select a joint to be "free" on the following basis.
1534⎇⎇We calculate the equivalent arm torque (Subsection 3.5) for
1535⎇⎇a unit force in the given direction or moment about the
1536⎇⎇given axis, and then normalize the torques by dividing
1537⎇⎇through by their respective F0 (Subsection 6.2). We then
1538⎇⎇select that joint which has the largest normalized torque to
1539⎇⎇be "free". This is the joint which is most sensitive to
1540⎇⎇motion in the required direction and it would be the first
1541⎇⎇joint to move if the force were slowly increased from zero
1542⎇⎇in the free direction.
1543⎇⎇If we require more degrees of freedom we repeat the process
1544⎇⎇being careful not to select the same joint twice.
1545⎇⎇To free the joint during motion, the feedback gains ke and
1546⎇⎇kv are set to zero (see Subsection 6.1). This means that the
1547⎇⎇free joint still has acceleration compensation (Equation
1548⎇⎇6.6) and gravity compensation, (Equation 6.5) If the hand is
1549⎇⎇required to exert an external force then this is also added
1550⎇⎇so that the joint is compensated for all known forces and
1551⎇⎇has no position directed feedback.
1552⎇⎇July 20, 1972␈→60⎇-1000⎇⎇6.3 PARTIAL␈←
1553⎇⎇␈→60⎇0⎇⎇ 16513⎇␈←␈→60⎇-1000⎇⎇␈←
1554⎇⎇The free joint servo response is then from (Equation 6.6)
1555⎇⎇␈→60⎇0⎇⎇E(s) = Te/( J * s␈↑2␈↓ )␈←␈→60⎇-1000⎇⎇[Eq. 6.17]␈←
1556⎇⎇In such actions as pushing a peg into a hole the hand must
1557⎇⎇have two degrees of freedom as the peg and hole represent
1558⎇⎇two constraints.
1559⎇⎇July 20, 1972␈→60⎇-1000⎇⎇␈←
1560⎇⎇␈→60⎇0⎇⎇ 16514⎇␈←␈→60⎇-1000⎇⎇␈←
1561⎇⎇␈→60⎇0⎇⎇7.0 CONTROL␈←
1562⎇⎇In addition to moving, the arm can perform such functions as
1563⎇⎇opening and closing its hand. These functions and motions
1564⎇⎇are called "primitives" and can be put together to make an
1565⎇⎇"arm program." An arm program is assembled to make a
1566⎇⎇trajectory file specifying the primitives together with
1567⎇⎇supporting data, trajectories, effective inertia constants
1568⎇⎇and gravity loading terms.
1569⎇⎇Two programs exist, one for assembling "arm programs" and
1570⎇⎇the other for executing the resulting trajectory files.
1571⎇⎇7.1 ARM STATE
1572⎇⎇The state of the arm is described by the following global
1573⎇⎇variables, which are located in the upper segment and are
1574⎇⎇available to all other programs sharing this segment, such
1575⎇⎇as a strategy program. At the termination of execution of a
1576⎇⎇program the state variables which describe the arm are
1577⎇⎇updated.
1578⎇⎇ ARM␈π_MOTION The name of the program currently being
1579⎇⎇ executed. This is a warning flag to other
1580⎇⎇ programs that the arm is in motion.
1581⎇⎇ ARM␈π_WAIT The name of the program, execution of which
1582⎇⎇ is temporarily suspended.
1583⎇⎇ ARM␈π_STATUS The error state of the arm at the end of
1584⎇⎇ execution.
1585⎇⎇ ARM␈π_LINK The 4x4 transform which describes the
1586⎇⎇ position and orientation of the hand at the
1587⎇⎇ end of execution.
1588⎇⎇ GRASP The separation between the finger tips.
1589⎇⎇ ARM␈π_PC The address of the next primitive to be
1590⎇⎇ executed in the current ARM␈π_SEGMENT.
1591⎇⎇ ARM␈π_SEGMENT An integer variable incremented at the
1592⎇⎇ beginning and end of each MOVE primitive.
1593⎇⎇Programs can be executed by the following two procedures.
1594⎇⎇July 20, 1972␈→60⎇-1000⎇⎇7.1 STATE␈←
1595⎇⎇␈→60⎇0⎇⎇ 16517⎇␈←␈→60⎇-1000⎇⎇␈←
1596⎇⎇ DO␈π_IT (NAME) causes the program NAME to be executed.
1597⎇⎇ DO␈π_PROCEED causes continued execution of the program
1598⎇⎇ whose execution was interrupted.
1599⎇⎇During execution various errors can occur; they are known as
1600⎇⎇errors because they cause the arm to stop and to apply all
1601⎇⎇the brakes, although some errors may be desired states.
1602⎇⎇There are various touch sensors on the arm and if, when
1603⎇⎇selected, one of them touchs anything, an "error" occurs.
1604⎇⎇The following is a list of the error messages:
1605⎇⎇ x1 Excessive force at joint x.
1606⎇⎇ 2 Hand closed beyond minimum specified opening
1607⎇⎇ x6 Touch sensor x was touched.
1608⎇⎇ 22 Excessive force at hand
1609⎇⎇ 23 Arm failed to stop on specified force.
1610⎇⎇There are other messages which refer to system errors, such
1611⎇⎇as file not found.
1612⎇⎇If an error occurs, execution of the program is suspended
1613⎇⎇and the state variables are updated, the contents of
1614⎇⎇ARM␈π_MOTION and ARM␈π_WAIT are exchanged and the error code is
1615⎇⎇set in ARM␈π_STATUS. Execution can proceed with the next
1616⎇⎇primitive, after erorr recovery, by calling procedure
1617⎇⎇DO␈π_PROCEED.
1618⎇⎇7.2 PRIMITIVES
1619⎇⎇This section lists the arm primitives, which have meaning at
1620⎇⎇two times: once at assembly when the trajectory file is
1621⎇⎇being created, when feasibility must be checked,
1622⎇⎇trajectories planned etc., and once at execution time when
1623⎇⎇the primitives are executed in the same way that
1624⎇⎇instructions are executed in a computer.
1625⎇⎇ OPEN (DIST) Open or close the hand such that the gap
1626⎇⎇ between the finger tips is DIST.
1627⎇⎇July 20, 1972␈→60⎇-1000⎇⎇7.2 PRIMITIVES␈←
1628⎇⎇␈→60⎇0⎇⎇ 16519⎇␈←␈→60⎇-1000⎇⎇␈←
1629⎇⎇ CLOSE (MINIMUM) Close the hand until it stops
1630⎇⎇ closing and then check that the gap between
1631⎇⎇ the finger tips is greater than MINIMUM. If
1632⎇⎇ it is less then give error 2.
1633⎇⎇ CHANGE (DX␈π_DY␈π_DZ, VELOCITY) Move the arm
1634⎇⎇ differentially (Subsection 5.5) to achieve a
1635⎇⎇ change of hand position of vector DX␈π_DY␈π_DZ
1636⎇⎇ at a maximum speed of VELOCITY.
1637⎇⎇ SWEEP (DIST, VELOCITY) Move the hand differentially
1638⎇⎇ (Subsection 5.5) in the direction of the
1639⎇⎇ hand's orientation vector a distance DIST at
1640⎇⎇ a maximum velocity VELOCITY.
1641⎇⎇ LIFT (DIST, VELOCITY) This is the same as SWEEP
1642⎇⎇ except that it is in the direction of O␈-1⎇␈π_ x A␈-1⎇␈π_
1643⎇⎇ where O and A are the orientation and
1644⎇⎇ approach vectors respectively.
1645⎇⎇ REACH (DIST, VELOCITY) Once again this is the same
1646⎇⎇ as SWEEP except in this case it is in the
1647⎇⎇ direction of the Approach vector.
1648⎇⎇ TILT (ANGLE) Rotate the hand differentially
1649⎇⎇ Subsection 5.5 about the orientation vector
1650⎇⎇ ANGLE degrees.
1651⎇⎇ TURN (ANGLE) This is a similar rotation about O␈-1⎇␈π_ x A␈-1⎇␈π_.
1652⎇⎇ TWIST (ANGLE) Here the rotation is about the
1653⎇⎇ approach vector.
1654⎇⎇ PLACE Move the hand vertically down until the hand
1655⎇⎇ meets some resistance, that is, the minimum
1656⎇⎇ resistance that the arm can reliably detect.
1657⎇⎇ MOVE ( T ) At assembly time check that the position
1658⎇⎇ specified by the hand transformation T is
1659⎇⎇ clear. Move the hand along a trajectory from
1660⎇⎇ its present position to | T |. The hand is
1661⎇⎇ moved up through a point ␈_LIFTOFF␈≡ given by
1662⎇⎇ ␈_␈_LIFTOFF␈≡ = ␈_INITIAL␈π_POSITION␈≡ + ␈_DEPART␈≡. where
1663⎇⎇ ␈_␈_DEPART␈≡ is a global vector initialized to z =
1664⎇⎇ 3 inches. Similarly on arrival the hand is
1665⎇⎇ moved down through a point ␈_SET␈π_DOWN␈≡ given
1666⎇⎇ by: ␈_SET␈π_DOWN␈≡ = ␈_FINAL␈π_POSITION␈≡ + ␈_ARRIVE␈≡.
1667⎇⎇ ␈_␈_ARRIVE␈≡ is also set to z = 3 inches.
1668⎇⎇July 20, 1972␈→60⎇-1000⎇⎇7.2 PRIMITIVES␈←
1669⎇⎇␈→60⎇0⎇⎇ 16520⎇␈←␈→60⎇-1000⎇⎇␈←
1670⎇⎇ PARK Plan a move as in MOVE but to the "park"
1671⎇⎇ position.
1672⎇⎇ DRAW (␈_DX␈π_DY␈π_DZ␈≡, ␈_ROT␈π_AXIS␈≡, O␈-1⎇-1, ␈_CRANK␈≡, ␈_CRANK␈π_AXIS␈≡, O␈-1⎇-2,
1673⎇⎇ TIME, LOOPS, FORCE, NUMBER␈π_FREE,
1674⎇⎇ ␈_FREE␈π_VECTOR␈≡) This is a trajectory motion of
1675⎇⎇ the hand. The hand is at the end of a vector
1676⎇⎇ (␈_CRANK␈≡) which is rotated O␈-1⎇-2 degrees around
1677⎇⎇ an axis (␈_CRANK␈π_AXIS␈≡) as its origin is
1678⎇⎇ translated (␈_DX␈π_DY␈π_DZ␈≡). At the same time the
1679⎇⎇ hand is re-orientated about another axis
1680⎇⎇ ␈_(␈_ROT␈π_AXIS␈≡) O␈-1⎇-1 degrees. See Figure 7.1. If
1681⎇⎇ the end point is the same as the initial
1682⎇⎇ then looping may be specified (see
1683⎇⎇ Subsection 5.2) finally a number of degrees
1684⎇⎇ of freedom may be specified and an excess
1685⎇⎇ force to be applied during the program. With
1686⎇⎇ this primitive we can do almost anything!
1687⎇⎇There are also control primitives which specify how the
1688⎇⎇other primitives are to be carried out.
1689⎇⎇ STOP (FORCE, MOMENT) During the next arm motion stop
1690⎇⎇ the arm when the feedback force is greater
1691⎇⎇ than the equivalent joint force (Subsection
1692⎇⎇ 3.5) if the arm fails to stop for this
1693⎇⎇ reason before the end of the motion,
1694⎇⎇ generate error 23.
1695⎇⎇ SKIPE (ERROR) If error ERROR occured during the
1696⎇⎇ previous primitive then skip the next
1697⎇⎇ primitive.
1698⎇⎇ SKIPN (ERROR) if error ERROR occured during the
1699⎇⎇ previous primitive execute the next
1700⎇⎇ primitive otherwise skip the next primitive.
1701⎇⎇ JUMP (N) Jump ahead N primitives where N may also be
1702⎇⎇ negative.
1703⎇⎇ WAIT Stop execution and update the state variables
1704⎇⎇ and wait for a proceed command.
1705⎇⎇ TOUCH (MASK) Activate the touch sensors specified by
1706⎇⎇ mask for the next primitive.
1707⎇⎇ SAVE Save the differential deviation from the
1708⎇⎇July 20, 1972␈→60⎇-1000⎇⎇7.2 PRIMITIVES␈←
1709⎇⎇␈→60⎇0⎇⎇ 16522⎇␈←␈→60⎇-1000⎇⎇␈←
1710⎇⎇ trajectory set point. This can be caused by
1711⎇⎇ CHANGE type primitives.
1712⎇⎇ RESTORE Cause the arm to deviate from the trajectory
1713⎇⎇ set point at the end of the next motion by
1714⎇⎇ the deviation last saved.
1715⎇⎇7.3 ASSEMBLY PROGRAM
1716⎇⎇The assembly program has two modes of input, message
1717⎇⎇procedure or source file. We will describe the source file
1718⎇⎇input here although it should be kept in mind that a
1719⎇⎇strategy program would be able to execute the same
1720⎇⎇functions.
1721⎇⎇Planning normally begins from the present position and
1722⎇⎇proceeds from there. The planned state of the arm is kept
1723⎇⎇in an 4x4 transformation STATE. Thus when a series of moves
1724⎇⎇is planned it is always from STATE to the specified
1725⎇⎇transform T that moves are made. At the end of each
1726⎇⎇successful move STATE is updated. In order to start an
1727⎇⎇program a BEGIN pseudo-op must be assembled. This causes a
1728⎇⎇trajectory file to be named and specifies the initial state
1729⎇⎇of STATE. Similarly at the end of an program the files must
1730⎇⎇be closed. We will list the pseudo-ops:
1731⎇⎇ BEGIN (FILE, T) Open file FILE as the trajectory
1732⎇⎇ file and initialize STATE to transformation
1733⎇⎇ T.
1734⎇⎇ MERGE Merge the last assembled primitive with the
1735⎇⎇ preceeding motion primitive.
1736⎇⎇ END Close the trajectory file.
1737⎇⎇ MACRO (FILE) Causes input to the planning program to
1738⎇⎇ be switched to FILE until the end of FILE.
1739⎇⎇ This gives us macros without parameters The
1740⎇⎇ MACROs may be nested.
1741⎇⎇In the case of most primitives their parameters are either
1742⎇⎇vectors or transformations, in the case of another program
1743⎇⎇using the assembly program these vectors and transformations
1744⎇⎇would be data structures of that program. In the case of
1745⎇⎇source input we need to be able to define such data types
1746⎇⎇July 20, 1972␈→60⎇-1000⎇⎇7.3 ASSEMBLY␈←
1747⎇⎇␈→60⎇0⎇⎇ 16524⎇␈←␈→60⎇-1000⎇⎇␈←
1748⎇⎇and associate them with names, which will be given in the
1749⎇⎇primitive call. All names must be defined before an
1750⎇⎇primitive can be assembled.
1751⎇⎇Data types:
1752⎇⎇ TRANS (NAME, R, X, Y, Z, Ox, Oy, Oz) Set up a 4x4
1753⎇⎇ transformation NAME such that it has
1754⎇⎇ position x,y,z and orientation vector
1755⎇⎇ Ox,Oy,Oz and that the approach vector is
1756⎇⎇ rotated R degrees from the reference
1757⎇⎇ approach vector about the orientation vector
1758⎇⎇ Equation 4.1.
1759⎇⎇ VECT (NAME, x␈↓1␈↑, x␈↓2␈↑, x␈↓3␈↑, x␈↓4␈↑) Define a vector NAME of
1760⎇⎇ value x␈↓1␈↑,x␈↓2␈↑,x␈↓3␈↑,x␈↓4␈↑.
1761⎇⎇ MOVE␈π_INSTANCE (TI, TF, IP) This is partly a data
1762⎇⎇ type primitive as it sets up a series of
1763⎇⎇ transformations to move an object with
1764⎇⎇ transformation TI such that it has
1765⎇⎇ transformation TF using intermediate
1766⎇⎇ position IP if necessary. However it
1767⎇⎇ assembles all the move and hand primitives
1768⎇⎇ to accomplish the move.
1769⎇⎇ PROTOTYPE (OBJECT) this sets up the prototype of the
1770⎇⎇ body to be moved by MOVE␈π_INSTANCE.
1771⎇⎇The planning program is 30K and shares a 14K segment which
1772⎇⎇contains runtime routines and global data. Typical running
1773⎇⎇times are 1 sec to plan a move. and 4 sec for MOVE␈π_INSTANCE.
1774⎇⎇7.4 PROGRAMMING EXAMPLES
1775⎇⎇We will give some examples of hand programs to clarify the
1776⎇⎇use of primitives. The first example is to move the hand to
1777⎇⎇a position 20,30,1 and to pick-up an object, then to move it
1778⎇⎇to 40,20,2 and to place it on the table.
1779⎇⎇ BEGIN TRANSFER 0
1780⎇⎇ TRANS T
1781⎇⎇ 90 20 30 1 1 0 0 Set up a transform to
1782⎇⎇ position the hand.
1783⎇⎇July 20, 1972␈→60⎇-1000⎇⎇7.4 PROGRAMS␈←
1784⎇⎇␈→60⎇0⎇⎇ 16526⎇␈←␈→60⎇-1000⎇⎇␈←
1785⎇⎇ MOVE T
1786⎇⎇ CLOSE 0.5 If there is nothing here
1787⎇⎇ then an error will occur.
1788⎇⎇ TRANS T
1789⎇⎇ 90 40 20 2 1 0 0 Change the transform to
1790⎇⎇ the new position.
1791⎇⎇ MOVE T
1792⎇⎇ PLACE
1793⎇⎇ PARK
1794⎇⎇ OPEN 3
1795⎇⎇ MERGE The hand will open as it
1796⎇⎇ starts to move.
1797⎇⎇ END
1798⎇⎇The next example is to grasp an object without moving it in
1799⎇⎇case there is some error in its position. The hand is closed
1800⎇⎇with the touch sensors activated until a finger touches the
1801⎇⎇object. The hand is then "swept" and closed in 0.1 inch
1802⎇⎇steps until the other finger touches. The hand is then
1803⎇⎇closed.
1804⎇⎇ TOUCH 1
1805⎇⎇ OPEN -1 This will cause the hand to
1806⎇⎇ close with touch "on".
1807⎇⎇L2: SKIPE 6 Did the left finger touch?
1808⎇⎇ JUMP L1
1809⎇⎇ SWEEP -0.1 1 Yes move right.
1810⎇⎇ TOUCH 1
1811⎇⎇ OPEN -1 And close the hand again.
1812⎇⎇ SKIPE 16 Did the other finger touch?
1813⎇⎇ JUMP L2 No move right again.
1814⎇⎇ JUMP L3 Yes all done.
1815⎇⎇L1: SKIPE 16 Check that the right finger
1816⎇⎇ touched.
1817⎇⎇ JUMP L8 No then some error.
1818⎇⎇ SWEEP 0.1 1 Move left.
1819⎇⎇ TOUCH 1
1820⎇⎇ OPEN -1 And close the hand again.
1821⎇⎇ SKIPE 6 Did the left finger touch?
1822⎇⎇ JUMP L1 No move again
1823⎇⎇L3: CLOSE 0.5 Yes close the hand.
1824⎇⎇ JUMP L10 And finish.
1825⎇⎇L8: WAIT The error state.
1826⎇⎇L10:
1827⎇⎇July 20, 1972␈→60⎇-1000⎇⎇7.4 PROGRAMS␈←
1828⎇⎇␈→60⎇0⎇⎇ 16527⎇␈←␈→60⎇-1000⎇⎇␈←
1829⎇⎇The last example, whose file name is CATCH, has no begin and
1830⎇⎇end as we are going to use it as a macro. We will use CATCH
1831⎇⎇first to position the hand on the object, then to turn it 90
1832⎇⎇degrees and close the hand again, so that the object will be
1833⎇⎇picked up centrally.
1834⎇⎇ MACRO CATCH Here we assume that the
1835⎇⎇ hand has been positioned
1836⎇⎇ over the object. The
1837⎇⎇ macro call will substitute
1838⎇⎇ the text from our previous
1839⎇⎇ example.
1840⎇⎇ OPEN 3
1841⎇⎇ TWIST 90 Now open the hand and turn
1842⎇⎇ it around 90 degrees.
1843⎇⎇ MACRO CATCH And close it again.
1844⎇⎇This we will also use as a macro called PICK. If the
1845⎇⎇primitive MACRO PICK were to replace the CLOSE 0.5 primitive
1846⎇⎇in the first example then we would have accomplished the
1847⎇⎇move but without having disturbed the object in its initial
1848⎇⎇position. We would also have located the object centrally.
1849⎇⎇We will give one final example, that is to put a nut, which
1850⎇⎇we will assume the hand to be holding, on a vertical bolt
1851⎇⎇located at 20,30,1 We will stop turning when the torque is
1852⎇⎇200 oz.in.
1853⎇⎇ BEGIN SCREW 0
1854⎇⎇ TRANS T
1855⎇⎇ 90 20 30 2 1 0 0
1856⎇⎇ MOVE T
1857⎇⎇ PLACE Move the nut to the bolt
1858⎇⎇ and place it.
1859⎇⎇ VECT ROTATION The bolt axis
1860⎇⎇ 0 0 -1
1861⎇⎇ VECT NULL
1862⎇⎇ 0 0 0
1863⎇⎇ VECT MOMENT The stopping torque.
1864⎇⎇ STOP NULL MOMENT Stop the arm during the
1865⎇⎇ motion when the torque
1866⎇⎇ is 200 oz. in.
1867⎇⎇ VECT FORCE
1868⎇⎇ 0 0 -20 We will want to push
1869⎇⎇ down on the bolt as we
1870⎇⎇ turn.
1871⎇⎇July 20, 1972␈→60⎇-1000⎇⎇7.4 PROGRAMS␈←
1872⎇⎇␈→60⎇0⎇⎇ 16528⎇␈←␈→60⎇-1000⎇⎇␈←
1873⎇⎇ VECT FREEX
1874⎇⎇ 1 0 0 The hand must be free in
1875⎇⎇ the x,y and z directions
1876⎇⎇ as we screw.
1877⎇⎇ VECT FREEY
1878⎇⎇ 0 1 0
1879⎇⎇ VECT FREEZ
1880⎇⎇ 0 0 -1
1881⎇⎇ DRAW
1882⎇⎇ NULL ROTATION 360 No change of position
1883⎇⎇ NULL NULL 0 No crank axis
1884⎇⎇ 60 3 FORCE 1 Second per turn, plan
1885⎇⎇ for a maximum of three
1886⎇⎇ turns. Exert the force.
1887⎇⎇ 3 Three drgrees of freedom.
1888⎇⎇ FREEX
1889⎇⎇ FREEY
1890⎇⎇ FREEZ
1891⎇⎇ END
1892⎇⎇This will screw the nut on the bolt.
1893⎇⎇7.5 EXECUTE
1894⎇⎇With the exception of MOVE and DRAW, which require
1895⎇⎇July 20, 1972␈→60⎇-1000⎇⎇7.5 EXECUTE␈←
1896⎇⎇␈→60⎇0⎇⎇ 16530⎇␈←␈→60⎇-1000⎇⎇␈←
1897⎇⎇trajectory files, most functions can be executed directly by
1898⎇⎇prefixing the primitive name by "DO." The assembly program
1899⎇⎇plans the action and sends it to the arm program to be
1900⎇⎇executed. This does not change the state of the arm program
1901⎇⎇if it is in a "wait" state and execution can continue after
1902⎇⎇any number of executed primitives. This method is used by
1903⎇⎇the interactive programs, which will plan a move to bring
1904⎇⎇the hand close to the required place and then plan a "wait."
1905⎇⎇When executed the hand position will be modified during the
1906⎇⎇wait phase by the interacting program executing a series of
1907⎇⎇"DO" commands. Execution of the preplanned trajectory can
1908⎇⎇then continue by executing a "DO␈π_PROCEED" command.
1909⎇⎇7.6 ARM PROGRAM
1910⎇⎇A simplified flow chart for the execution program is shown
1911⎇⎇in Figure 7.2. The loop is executed 60 times a second. If
1912⎇⎇the arm is not in motion then RUN is false and the touch
1913⎇⎇sensors are checked before performing any function. At the
1914⎇⎇completion of a motion or, if the arm is not moving at the
1915⎇⎇completion of a function, the program counter is incremented
1916⎇⎇and the next primitive executed. A zero primitive terminates
1917⎇⎇execution.
1918⎇⎇The block ANGLES measures all the joint angles and performs
1919⎇⎇a piecewise non-linear conversion on them. The velocities
1920⎇⎇are also determined, either by reading the tachometer
1921⎇⎇generator outputs (joints 1,2) or by differencing the
1922⎇⎇position information.
1923⎇⎇The block SERVO corresponds to Subsection 6.1; here the
1924⎇⎇errors are computed and the drive torques calculated, If a
1925⎇⎇stop arm primitive is in effect then the error torques are
1926⎇⎇checked against the equivalent arm torques to determine if
1927⎇⎇the arm should be stopped. At the end of the trajectory the
1928⎇⎇position errors are nulled. When each individual joint is
1929⎇⎇within the error tolerance the brake is applied, and when
1930⎇⎇all joints are stopped the next primitive is executed.
1931⎇⎇Section DRIVE takes as input the joint torques and computes
1932⎇⎇the motor drive. It checks for excessive force and stops the
1933⎇⎇arm.
1934⎇⎇The THETA section computes the next values of the joint
1935⎇⎇angles. interpolates the Ji and Tg values (see Equation
1936⎇⎇6.12) and controls the switching as required for looping.
1937⎇⎇July 20, 1972␈→60⎇-1000⎇⎇7.6 ARM␈←
1938⎇⎇␈→60⎇0⎇⎇ 16533⎇␈←␈→60⎇-1000⎇⎇␈←
1939⎇⎇The Touch sensors are then checked and, if any, have been
1940⎇⎇touched the arm is stopped. Finally the function if any is
1941⎇⎇performed. Functions are not normally performed while
1942⎇⎇running with the exception of hand opening.
1943⎇⎇The execution time for the loop is approximately 9msec on
1944⎇⎇the PDP-6 processor, using floating point hardware. The
1945⎇⎇program length is 3K words including trajectory buffers.
1946⎇⎇While the arm is running the trajectory set point O␈-1⎇-t is
1947⎇⎇given by:
1948⎇⎇␈→60⎇0⎇⎇O␈-1⎇-t = f(t)␈←␈→60⎇-1000⎇⎇[Eq. 7.1]␈←
1949⎇⎇where f(t) is the appropriate part trajectory polynomial
1950⎇⎇Subsection 5.2 The arm set point is as follows:
1951⎇⎇␈→60⎇0⎇⎇O␈-1⎇-s = O␈-1⎇-t + dO␈-1⎇-␈←␈→60⎇-1000⎇⎇[Eq. 7.2]␈←
1952⎇⎇where O␈-1⎇-t is a constant offset between the set point and the
1953⎇⎇trajectory point. Between liftoff and set-down for a period
1954⎇⎇of time Tm:
1955⎇⎇␈→60⎇0⎇⎇O␈-1⎇-s = O␈-1⎇-t + dO␈-1⎇- +d2O␈-1⎇-*g(t)␈←␈→60⎇-1000⎇⎇[Eq. 7.3]␈←
1956⎇⎇where g(t) is given in Equation 5.38. When t = Tm dO␈-1⎇- is
1957⎇⎇changed as follows:
1958⎇⎇␈→60⎇0⎇⎇dO␈-1⎇- ← dO␈-1⎇- + d2O␈-1⎇-␈←␈→60⎇-1000⎇⎇[Eq. 7.4]␈←
1959⎇⎇␈→60⎇0⎇⎇d2O␈-1⎇- ← 0␈←␈→60⎇-1000⎇⎇[Eq. 7.5]␈←
1960⎇⎇At the beginning of each trajectory motion we set:
1961⎇⎇␈→60⎇0⎇⎇dO␈-1⎇- ← O␈-1⎇- - O␈-1⎇-t␈←␈→60⎇-1000⎇⎇[Eq. 7.6]␈←
1962⎇⎇␈→60⎇0⎇⎇d2O␈-1⎇- ← -dO␈-1⎇- + d2O␈-1⎇-␈←␈→60⎇-1000⎇⎇[Eq. 7.7]␈←
1963⎇⎇where O␈-1⎇- is the observed value. Thus at the beginning of a
1964⎇⎇trajectory:
1965⎇⎇␈→60⎇0⎇⎇O␈-1⎇-s = O␈-1⎇- = O␈-1⎇-t + dO␈-1⎇-␈←␈→60⎇-1000⎇⎇[Eq. 7.8]␈←
1966⎇⎇and at the end of let-down:
1967⎇⎇␈→60⎇0⎇⎇O␈-1⎇-s = O␈-1⎇-t + d2O␈-1⎇-i␈←␈→60⎇-1000⎇⎇[Eq. 7.9]␈←
1968⎇⎇where d2O␈-1⎇-i is the value of d2O␈-1⎇- at the beginning of the
1969⎇⎇July 20, 1972␈→60⎇-1000⎇⎇7.6 ARM␈←
1970⎇⎇␈→60⎇0⎇⎇ 16534⎇␈←␈→60⎇-1000⎇⎇␈←
1971⎇⎇trajectory Equation 7.7. If d2O␈-1⎇- was set to zero at the end
1972⎇⎇of the previous trajectory by Equation 7.5 then Equation 7.9
1973⎇⎇becomes:
1974⎇⎇␈→60⎇0⎇⎇O␈-1⎇-s = O␈-1⎇-t␈←␈→60⎇-1000⎇⎇[Eq. 7.10]␈←
1975⎇⎇This means that if the arm is displaced from the point from
1976⎇⎇which the trajectory was planned, the arm is gradually
1977⎇⎇brought back to the trajectory during the mid-part of the
1978⎇⎇motion.
1979⎇⎇All the differential motions are accomplished by loading d2O␈-1⎇-
1980⎇⎇setting Tm and seting RUN, then at the end of Tm the arm has
1981⎇⎇moved d2O␈-1⎇- and the value of dO␈-1⎇- has also been incremented by
1982⎇⎇d2O␈-1⎇-.
1983⎇⎇The save command causes dO␈-1⎇- to be saved and the restore
1984⎇⎇primitive loads d2O␈-1⎇- with the previously stored dO␈-1⎇-. By this
1985⎇⎇means we can find some location by differential motion, save
1986⎇⎇the dO␈-1⎇-, and then if we wish to return to this differentially
1987⎇⎇modified position we restore dO␈-1⎇- into d2O␈-1⎇- before returning to
1988⎇⎇the unmodified position.
1989⎇⎇July 20, 1972␈→60⎇-1000⎇⎇␈←
1990⎇⎇␈→60⎇0⎇⎇ 16535⎇␈←␈→60⎇-1000⎇⎇␈←
1991⎇⎇␈→60⎇0⎇⎇8.0 MOTION ANALYSIS␈←
1992⎇⎇8.1 FORCE AND RESULTANT MOTION ANALYSIS
1993⎇⎇July 20, 1972␈→60⎇-1000⎇⎇␈←
1994⎇⎇␈→60⎇0⎇⎇ 16538⎇␈←␈→60⎇-1000⎇⎇␈←
1995⎇⎇␈→60⎇0⎇⎇9.0 CONCLUSIONS␈←
1996⎇⎇9.1 CONCLUSIONS AND FUTURE WORK
1997⎇⎇July 20, 1972 ␈→60⎇-1000⎇⎇␈←
1998⎇⎇␈→60⎇0⎇⎇ 16545⎇␈←␈→60⎇-1000⎇⎇␈←
1999⎇⎇␈→60⎇0⎇⎇APPENDIX␈←
2000⎇⎇A.1 HARDWARE DESCRIPTION
2001⎇⎇The arm is shown in Figure 3.1, we will give its esential
2002⎇⎇features here. The hand, a simple parallel jaw hand of 10
2003⎇⎇cm opening, can be positioned anywhere in a work space of 1
2004⎇⎇meter radius about the shoulder and with any orientation. It
2005⎇⎇is provided with elementary "switch" type touch sensors.
2006⎇⎇The arm, designed to work with 5kg loads, is powered by
2007⎇⎇electric motors; harmonic drive gear reductions are employed
2008⎇⎇on most joints. These reductions are efficient which means
2009⎇⎇that the arm can react to external forces (see Subsection
2010⎇⎇6.3). and thus the hand can follow an externally
2011⎇⎇constrained motion. Brakes are provided to hold the arm in
2012⎇⎇position when it is stopped, so that the arm does not have
2013⎇⎇to be continuously servoed.
2014⎇⎇The power to weight ratio is high, resulting in a very high
2015⎇⎇performance arm. It is possible to move the arm 180 degrees
2016⎇⎇at the shoulder in little over a second including stopping
2017⎇⎇and starting time. The absolute accuracy of the arm is of
2018⎇⎇the order of ␈π-0.1 inches and its repeatability ␈π-0.03 inches.
2019⎇⎇Point to point servoing usually takes in the order of 1 to 2
2020⎇⎇seconds.
2021⎇⎇Joint angle is measured by integral potentiometers and is
2022⎇⎇read into the computer by a 12 bit A/D converter. In the
2023⎇⎇case of joint 6 where continuous motion is possible two
2024⎇⎇wipers are provided on a common element. Whenever the
2025⎇⎇current wiper is within 1/8th of the end of scale the other
2026⎇⎇wiper is read. As wipers are interchanged an offset is added
2027⎇⎇when appropriate. This continuous motion is used in such
2028⎇⎇tasks as screwing in screws.
2029⎇⎇Control of the arm is by means of a voltage pulse width
2030⎇⎇modulated signal, the polarity and duration are set by the
2031⎇⎇computer. If the arm is not addressed by the computer once
2032⎇⎇every 20 msec. a hardware interlock automatically stops the
2033⎇⎇motors and puts on the brakes. This must be "unlocked" by
2034⎇⎇the computer before the arm can be run again.
2035⎇⎇July 20, 1972␈→60⎇-1000⎇⎇A.2 SAIL␈←
2036⎇⎇␈→60⎇0⎇⎇ 16549⎇␈←␈→60⎇-1000⎇⎇␈←
2037⎇⎇A.2 SAIL
2038⎇⎇SAIL is an extended form of ALGOL with LEAP added to it.
2039⎇⎇LEAP provides the ITEM and DATUM constructs which we use to
2040⎇⎇represent the models. Bodies and parts of them (faces,
2041⎇⎇vertices, edges) are represented by items, a data type which
2042⎇⎇is treated simply as a name.
2043⎇⎇For example in the expression:
2044⎇⎇␈→60⎇0⎇⎇I←I+5;␈←␈→60⎇-1000⎇⎇[Eq. 10.1]␈←
2045⎇⎇we are referring to the value of I, In the case of ITEMS we
2046⎇⎇refer to the name.
2047⎇⎇The main use of items is that they may be associated
2048⎇⎇together in the following manner:
2049⎇⎇␈→60⎇0⎇⎇Attribute of Object is Value ␈←␈→60⎇-1000⎇⎇[Eq. 10.2]␈←
2050⎇⎇Where Attribute, Object and Value are items.
2051⎇⎇To associate three items the "MAKE" construct is used.
2052⎇⎇␈→60⎇0⎇⎇MAKE A⊗O≡V ␈←␈→60⎇-1000⎇⎇[Eq. 10.3]␈←
2053⎇⎇Where the "⊗" stands for "of" and the "≡" stands for "is".
2054⎇⎇To delete such an association the "ERASE" construct is used.
2055⎇⎇␈→60⎇0⎇⎇ERASE A⊗O≡V ␈←␈→60⎇-1000⎇⎇[Eq. 10.4]␈←
2056⎇⎇There exists a mechanism for searching the store of
2057⎇⎇associations in an efficient manner, the "FOREACH"
2058⎇⎇construct. Assume that we had MADE the following
2059⎇⎇associations:
2060⎇⎇␈→60⎇0⎇⎇FACE⊗CUBE≡FACE1␈←
2061⎇⎇␈→60⎇0⎇⎇FACE⊗CUBE≡FACE2␈←
2062⎇⎇Then the following FOREACH statement:
2063⎇⎇␈→60⎇0⎇⎇FOREACH F| FACE⊗CUBE≡F DO <statement> ␈←␈→60⎇-1000⎇⎇[Eq. 10.5]␈←
2064⎇⎇where <statement> is an ALGOL statement and F is an item
2065⎇⎇July 20, 1972␈→60⎇-1000⎇⎇A.2 SAIL␈←
2066⎇⎇␈→60⎇0⎇⎇ 16550⎇␈←␈→60⎇-1000⎇⎇␈←
2067⎇⎇variable and will be progressively assigned to FACE1 and
2068⎇⎇then to FACE2, the statement being executed each time.
2069⎇⎇One additional piece of data can be associated with an item
2070⎇⎇known as its DATUM. This is usually of algebraic type, for
2071⎇⎇example an array. In order to refer to this array by name
2072⎇⎇the DATUM construct is used, and to refer to an element of
2073⎇⎇the array the subscript list is added.
2074⎇⎇␈→60⎇0⎇⎇DATUM(A) ␈←␈→60⎇-1000⎇⎇[Eq. 10.6]␈←
2075⎇⎇␈→60⎇0⎇⎇DATUM(A)[1,3]␈←
2076⎇⎇Thus if we represent a vertex of a body by an item e.g.
2077⎇⎇VERTEX1 then we may give as its datum the vector
2078⎇⎇representing the position of the vertex, where
2079⎇⎇DATUM(VERTEX1)[1] would be for instance the "x" coordinate.
2080⎇⎇A.3 VECTORS AND TRANSFORMATIONS
2081⎇⎇Vectors representing points in space are denoted by an
2082⎇⎇under-bar "V␈-1⎇␈π_" and are described by four components:
2083⎇⎇␈→60⎇0⎇⎇| V[1] V[2] V[3] V[4]|␈←␈→60⎇-1000⎇⎇[Eq. 10.7]␈←
2084⎇⎇Such that the components of the vector V␈-1⎇␈π_ along the x,y,z
2085⎇⎇axes is given by:
2086⎇⎇␈→60⎇0⎇⎇X = V[1]/V[4]␈←␈→60⎇-1000⎇⎇[Eq. 10.8]␈←
2087⎇⎇␈→60⎇0⎇⎇Y = V[2]/V[4]␈←
2088⎇⎇July 20, 1972␈→60⎇-1000⎇⎇A.3 VECTORS␈←
2089⎇⎇␈→60⎇0⎇⎇ 16552⎇␈←␈→60⎇-1000⎇⎇␈←
2090⎇⎇␈→60⎇0⎇⎇Z = V[3]/V[4]␈←
2091⎇⎇With this scheme both the null vector and vectors at
2092⎇⎇infinity are representable:
2093⎇⎇␈→60⎇0⎇⎇| 0 0 0 1 |␈←␈→60⎇-1000⎇⎇[Eq. 10.9]␈←
2094⎇⎇␈→60⎇0⎇⎇| V[1] V[2] V[3] 0 |␈←␈→60⎇-1000⎇⎇[Eq. 10.10]␈←
2095⎇⎇All usual vector operations are defined. Addition and
2096⎇⎇subtraction where:
2097⎇⎇␈→60⎇0⎇⎇R␈-1⎇␈π_ ← ( A␈-1⎇␈π_ ␈π- B␈-1⎇␈π_ )␈←␈→60⎇-1000⎇⎇[Eq. 10.11]␈←
2098⎇⎇␈→60⎇0⎇⎇R[1] = A[1]/A[4] ␈π- B[1]/B[4]␈←
2099⎇⎇␈→60⎇0⎇⎇R[2] = A[2]/A[4] ␈π- B[2]/B[4]␈←
2100⎇⎇␈→60⎇0⎇⎇R[3] = A[3]/A[4] ␈π- B[3]/B[4]␈←
2101⎇⎇␈→60⎇0⎇⎇R[4] = 1.0␈←
2102⎇⎇The dot product where:
2103⎇⎇␈→60⎇0⎇⎇( A␈-1⎇␈π_ . B␈-1⎇␈π_ ) = ( A[1]B[1] + A[2]B[2] + A[3]B[3] )/A[4]B[4]␈←
2104⎇⎇␈→60⎇0⎇⎇␈←␈→60⎇-1000⎇⎇[Eq. 10.12]␈←
2105⎇⎇The cross product where: R␈-1⎇␈π_ ← (A␈-1⎇␈π_ X B␈-1⎇␈π_ )␈→60⎇-1000⎇⎇[Eq. 10.13]␈←
2106⎇⎇␈→60⎇0⎇⎇R[1] = A[2]*B[3] - B[2]*A[3]␈←
2107⎇⎇␈→60⎇0⎇⎇R[2] = A[3]*B[1] - B[3]*A[1]␈←
2108⎇⎇July 20, 1972␈→60⎇-1000⎇⎇A.3 VECTORS␈←
2109⎇⎇␈→60⎇0⎇⎇ 16553⎇␈←␈→60⎇-1000⎇⎇␈←
2110⎇⎇␈→60⎇0⎇⎇R[3] = A[1]*B[2] - B[1]*A[2]␈←
2111⎇⎇␈→60⎇0⎇⎇R[4] = A[4]*B[4]␈←
2112⎇⎇Magnitude ␈→60⎇10⎇⎇| A␈-1⎇␈π_ | = 1/A[4](A[1]␈↑2␈↓ + A[2]␈↑2␈↓ +A[3]␈↑2␈↓ )␈↑1/2␈↓␈←␈→60⎇-1000⎇⎇[Eq.␈←
2113⎇⎇␈→60⎇-1000⎇⎇10.14]␈←
2114⎇⎇Scale ␈→60⎇6⎇⎇sA␈-1⎇␈π_ = | A[1] A[2] A[3] A[4]/s |␈←␈→60⎇-1000⎇⎇[Eq. 10.15]␈←
2115⎇⎇Planes are also represented by four components
2116⎇⎇␈→60⎇0⎇⎇| F[1] F[2] F[3] F[4] |␈←␈→60⎇-1000⎇⎇[Eq. 10.16]␈←
2117⎇⎇but in this case the first three components represent the
2118⎇⎇outward pointing normal of the plane normalized to unity,
2119⎇⎇and the fourth component represents the negative directed
2120⎇⎇distance to the plane in the direction of the normal from
2121⎇⎇the origin. See Figure 10.1
2122⎇⎇␈→60⎇0⎇⎇F[1] = N[1]␈←␈→60⎇-1000⎇⎇[Eq. 10.17]␈←
2123⎇⎇␈→60⎇0⎇⎇F[2] = N[2]␈←
2124⎇⎇␈→60⎇0⎇⎇F[3] = N[3]␈←
2125⎇⎇␈→60⎇0⎇⎇F[4] = -D␈←
2126⎇⎇If a plane is so represented, a simple test exists to
2127⎇⎇determine if a point V␈-1⎇␈π_ lies inside or outside the plane. We
2128⎇⎇form the inner product.
2129⎇⎇␈→60⎇0⎇⎇|F| * |V| = | V[1]F[1] + V[2]F[2] + V[3]F[3] + V[4]F[4] |␈←
2130⎇⎇␈→60⎇0⎇⎇␈←␈→60⎇-1000⎇⎇[Eq. 10.18]␈←
2131⎇⎇July 20, 1972␈→60⎇-1000⎇⎇A.3 VECTORS␈←
2132⎇⎇␈→60⎇0⎇⎇ 16555⎇␈←␈→60⎇-1000⎇⎇␈←
2133⎇⎇and depending on the sign the point lies inside or outside
2134⎇⎇the plane, If the product is zero then the point lies in the
2135⎇⎇plane.
2136⎇⎇It is important to note the difference between the dot
2137⎇⎇product (Equation 10.12) and the inner product (Equation
2138⎇⎇10.18).
2139⎇⎇Having represented points and planes it remains to be able
2140⎇⎇to rotate and translate them. We do this by pre-multiplying
2141⎇⎇by a transformation matrix. In the case of points, a
2142⎇⎇rotation is represented by a four by four matrix:
2143⎇⎇ | xx' yx' zx' 0 |
2144⎇⎇ | T | = | xy' yy' zy' 0 |␈→60⎇-1000⎇⎇[Eq. 10.19]␈←
2145⎇⎇ | xz' yz' zz' 0 |
2146⎇⎇ | 0 0 0 1 |
2147⎇⎇Here the first three columns represent the components of the
2148⎇⎇unit vectors in the reference system. i.e. a unit vector
2149⎇⎇along the x axis
2150⎇⎇␈→60⎇0⎇⎇x␈-1⎇␈π_ = | 1 0 0 1 |␈←␈→60⎇-1000⎇⎇[Eq. 10.20]␈←
2151⎇⎇if then transformed by
2152⎇⎇␈→60⎇0⎇⎇| x'| = | T | * | x |␈←␈→60⎇-1000⎇⎇[Eq. 10.21]␈←
2153⎇⎇has components
2154⎇⎇␈→60⎇0⎇⎇x␈-1⎇␈π_' = | xx' xy' xz' 1 |␈←␈→60⎇-1000⎇⎇[Eq. 10.22]␈←
2155⎇⎇in the rotated system. (see Figure 10.2)
2156⎇⎇In the case of a translation we have the matrix:
2157⎇⎇ | 1 0 0 xt |
2158⎇⎇ | T | = | 0 1 0 yt |␈→60⎇-1000⎇⎇[Eq. 10.23]␈←
2159⎇⎇ | 0 0 1 zt |
2160⎇⎇ | 0 0 0 1 |
2161⎇⎇from which it can be seen that:
2162⎇⎇␈→60⎇0⎇⎇| T | * | V |␈←␈→60⎇-1000⎇⎇[Eq. 10.24]␈←
2163⎇⎇July 20, 1972␈→60⎇-1000⎇⎇A.3 VECTORS␈←
2164⎇⎇␈→60⎇0⎇⎇ 16557⎇␈←␈→60⎇-1000⎇⎇␈←
2165⎇⎇has components:
2166⎇⎇␈→60⎇0⎇⎇| V[1]+xt V[2]+yt V[3]+zt V[4] |␈←␈→60⎇-1000⎇⎇[Eq. 10.25]␈←
2167⎇⎇of the translated vector.
2168⎇⎇To be able to rotate and translate we multiply the
2169⎇⎇transformation matrices together:
2170⎇⎇␈→60⎇0⎇⎇| T | = | Tt| * | Tr|␈←␈→60⎇-1000⎇⎇[Eq. 10.26]␈←
2171⎇⎇ | xx' yx' zx' xt |
2172⎇⎇ | T | = | xy' yy' zy' yt |␈→60⎇-1000⎇⎇[Eq. 10.27]␈←
2173⎇⎇ | xz' yz' zz' zt |
2174⎇⎇ | 0 0 0 1 |
2175⎇⎇These are the only transformations that we will be
2176⎇⎇performing although such a system is capable of many more
2177⎇⎇transformations such as scaling, stretching and perspective.
2178⎇⎇In the case of planes we require Equation 10.18 to hold
2179⎇⎇under these transformations such that if:
2180⎇⎇␈→60⎇0⎇⎇| F | * | V | = | F'| * | V'| ␈←␈→60⎇-1000⎇⎇[Eq. 10.28]␈←
2181⎇⎇␈→60⎇0⎇⎇| V'| = | T | * | V |␈←␈→60⎇-1000⎇⎇[Eq. 10.29]␈←
2182⎇⎇Then if we substitute for | V | in Equation 10.28 we obtain
2183⎇⎇␈→60⎇0⎇⎇| F | * | V | = | F'| * | T | * | V |␈←␈→60⎇-1000⎇⎇[Eq. 10.30]␈←
2184⎇⎇and thus:
2185⎇⎇␈→60⎇0⎇⎇| F'| = | F | * | T |␈↑-1␈↓␈←␈→60⎇-1000⎇⎇[Eq. 10.31]␈←
2186⎇⎇␈→60⎇0⎇⎇| F'| = | T |␈↑-1␈↑T␈↓␈↓* | F |␈←␈→60⎇-1000⎇⎇[Eq. 10.32]␈←
2187⎇⎇Planes are thus transformed by pre-multiplying by the
2188⎇⎇inverse transform transposed.